ROS2 Interface

std_msgs/Header header
string handedness               # "left" or "right"
float32 system_temperature      # System temperature (°C)
float32 input_voltage           # Input voltage (V)
float32[20] joint_temperatures  # Joint temperatures (°C)
uint32[20] error_codes          # Error codes (0=normal)
bool[20] enabled                # Joint enabled status
float32[20] effort_limits       # Effort limit settings (A)

# Request
uint8 finger_id   # 0-4 or 255 (all fingers)
uint8 joint_id    # 0-3 or 255 (all joints)
bool enabled      # true=enable, false=disable
---
# Response
bool success
string message

# Request
uint8 finger_id   # 0-4 or 255 (all fingers)
uint8 joint_id    # 0-3 or 255 (all joints)
---
# Response
bool success
string message

# Disable all joints
ros2 service call /hand_0/set_enabled wujihand_msgs/srv/SetEnabled \
  "{finger_id: 255, joint_id: 255, enabled: false}"

# Enable all joints
ros2 service call /hand_0/set_enabled wujihand_msgs/srv/SetEnabled \
  "{finger_id: 255, joint_id: 255, enabled: true}"

# Enable only the index finger (finger_id=1)
ros2 service call /hand_0/set_enabled wujihand_msgs/srv/SetEnabled \
  "{finger_id: 1, joint_id: 255, enabled: true}"

# Enable only the first joint of thumb
ros2 service call /hand_0/set_enabled wujihand_msgs/srv/SetEnabled \
  "{finger_id: 0, joint_id: 0, enabled: true}"

# Reset all joint errors
ros2 service call /hand_0/reset_error wujihand_msgs/srv/ResetError \
  "{finger_id: 255, joint_id: 255}"

# Zero all joints
ros2 topic pub /hand_0/joint_commands sensor_msgs/msg/JointState \
  "{position: [0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0]}" --once

# Control only two joints of the index finger
ros2 topic pub /hand_0/joint_commands sensor_msgs/msg/JointState \
  "{name: ['right_finger2_joint2', 'right_finger2_joint3'], position: [1.0, 1.0]}" --once

# Home a single hand
ros2 launch wujihand_bringup home.launch.py

# Home both hands together over 1.5s (names match the topics wujihand_dual.launch.py publishes)
ros2 launch wujihand_bringup home.launch.py \
    hand_names:=hand_left,hand_right duration:=1.5

# Read joint states once (published at 1000Hz by default)
ros2 topic echo /hand_0/joint_states --once

# Read diagnostics (published at 10Hz by default)
ros2 topic echo /hand_0/hand_diagnostics --once

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import JointState

class WujiHandController(Node):
    """Dexterous hand control node for subscribing to states and publishing commands"""

    def __init__(self, hand_name='hand_0'):
        super().__init__('wujihand_controller')
        self.hand_name = hand_name
        self.count = 0  # Message counter for throttling output

        # Create publisher: send joint position commands to joint_commands topic
        self.cmd_pub = self.create_publisher(
            JointState,
            f'/{hand_name}/joint_commands',
            qos_profile_sensor_data
        )

        # Create subscriber: receive joint states from joint_states topic (1000Hz)
        self.state_sub = self.create_subscription(
            JointState,
            f'/{hand_name}/joint_states',
            self.state_callback,
            qos_profile_sensor_data
        )

    def state_callback(self, msg):
        """
        Joint state callback
        msg.position: Current positions of 20 joints (radians)
        msg.effort: Current effort of 20 joints (amperes)
        """
        self.count += 1
        if self.count % 100 == 0:  # Print every 100 messages (~10Hz)
            # Print the 4 joint positions of the thumb (indices 0-3)
            self.get_logger().info(f'Position: {msg.position[:4]}')

    def send_positions(self, positions):
        """
        Send joint position command
        positions: Array of 20 target positions for joints (radians)
        """
        msg = JointState()
        msg.position = list(positions)
        self.cmd_pub.publish(msg)

def main():
    # Initialize ROS2
    rclpy.init()

    # Create control node
    node = WujiHandController(hand_name='hand_0')

    # Send zero command to all joints (set all 20 joints to 0 radians)
    node.send_positions([0.0] * 20)

    # Enter event loop, continuously receive joint states
    rclpy.spin(node)

    # Cleanup
    rclpy.shutdown()

if __name__ == '__main__':
    main()

# Load ROS2 environment, replace <distro> with your installed ROS2 distribution
source /opt/ros/<distro>/setup.bash
# Navigate to driver workspace and run the following commands to source it (skip if installed via Deb)
cd <workspace>
source install/setup.bash
ros2 launch wujihand_bringup wujihand.launch.py

# Load ROS2 environment, replace <distro> with your installed ROS2 distribution
source /opt/ros/<distro>/setup.bash
python3 wujihand_controller.py

[INFO] [1769589384.755943411] [wujihand_controller]: Position: array('d', [0.048882766883356236, 0.007923863595842663, 0.03073836071041754, 0.03718776672849719])
[INFO] [1769589384.855927131] [wujihand_controller]: Position: array('d', [0.048882766883356236, 0.007923863595842663, 0.03073836071041754, 0.03718776672849719])
...

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

/**
 * Dexterous hand control node
 * Functions: subscribe to joint states, publish position commands
 */
class WujiHandController : public rclcpp::Node {
public:
    WujiHandController(const std::string& hand_name = "hand_0")
        : Node("wujihand_controller"), hand_name_(hand_name), count_(0)
    {
        // QoS configuration: use SensorDataQoS to match driver
        auto qos = rclcpp::SensorDataQoS();

        // Create publisher: send joint position commands to joint_commands topic
        cmd_pub_ = this->create_publisher<sensor_msgs::msg::JointState>(
            "/" + hand_name_ + "/joint_commands", qos);

        // Create subscriber: receive joint states from joint_states topic (1000Hz)
        state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "/" + hand_name_ + "/joint_states", qos,
            std::bind(&WujiHandController::state_callback, this, std::placeholders::_1));
    }

    /**
     * Send joint position command
     * @param positions Array of 20 target positions for joints (radians)
     */
    void send_positions(const std::vector<double>& positions) {
        auto msg = sensor_msgs::msg::JointState();
        msg.position = positions;
        cmd_pub_->publish(msg);
    }

private:
    /**
     * Joint state callback
     * msg->position: Current positions of 20 joints (radians)
     * msg->effort: Current effort of 20 joints (amperes)
     */
    void state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) {
        count_++;
        if (count_ % 100 == 0) {  // Print every 100 messages (~10Hz)
            RCLCPP_INFO(this->get_logger(), "Position: [%.3f, %.3f, %.3f, %.3f]",
                msg->position[0], msg->position[1], msg->position[2], msg->position[3]);
        }
    }

    std::string hand_name_;
    int count_;  // Message counter for throttling output
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr cmd_pub_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub_;
};

int main(int argc, char** argv) {
    // Initialize ROS2
    rclcpp::init(argc, argv);

    // Create control node
    auto node = std::make_shared<WujiHandController>("hand_0");

    // Send zero command to all joints (set all 20 joints to 0 radians)
    node->send_positions(std::vector<double>(20, 0.0));
    RCLCPP_INFO(node->get_logger(), "Sent zero position command");

    // Enter event loop, continuously receive joint states
    rclcpp::spin(node);

    // Cleanup
    rclcpp::shutdown();
    return 0;
}

cd ~/test
# Load ROS2 environment, replace <distro> with your installed ROS2 distribution
source /opt/ros/<distro>/setup.bash
ros2 pkg create --build-type ament_cmake test_wujihand --dependencies rclcpp sensor_msgs

add_executable(wujihand_controller src/wujihand_controller.cpp)
ament_target_dependencies(wujihand_controller rclcpp sensor_msgs)
install(TARGETS wujihand_controller DESTINATION lib/${PROJECT_NAME})

# Load ROS2 environment, replace <distro> with your installed ROS2 distribution
source /opt/ros/<distro>/setup.bash
# Navigate to driver workspace and run the following commands to source it (skip if installed via Deb)
cd <workspace>
source install/setup.bash
ros2 launch wujihand_bringup wujihand.launch.py

# Load ROS2 environment, replace <distro> with your installed ROS2 distribution
source /opt/ros/<distro>/setup.bash
cd ~/test
colcon build --packages-select test_wujihand
source ~/test/install/setup.bash
ros2 run test_wujihand wujihand_controller

[INFO] [1769591460.127643591] [wujihand_controller]: Sent zero position command
[INFO] [1769591460.591707131] [wujihand_controller]: Position: [0.043, 0.022, 0.034, 0.035]
[INFO] [1769591460.691719595] [wujihand_controller]: Position: [0.044, 0.012, 0.035, 0.034]
...