Create a ros2_control controller, xela_taxel_joint_state_publisher, that publishes a JointState stream specifically for the XELA Taxel sensor.
This controller filters incoming JointState messages or initializes joint positions from a configured list, then publishes a unified JointState topic.
This controller is designed to run within ros2_control and be managed by the controller_manager.

As a first step, this publisher supports the Allegrohand v4 (ros2_control type) and 2F-85 (uSPr2F) models.
It is spawned by the controller manager when the robot is started, and upon startup, it publishes the JointState of the Taxel sensor of the specified model.

Create xela_taxel_joint_state_publisher¶

xela_taxel_joint_state_publisher/
├── CMakeLists.txt
├── config
│   ├── list_2f_gripper_joint.yaml
│   ├── list_allegrohand_joint.yaml
│   └── xela_taxel_jsp_config.yaml
├── controller_plugins.xml
├── include
│   └── xela_taxel_joint_state_publisher
│       └── xela_taxel_joint_state_publisher.hpp
├── package.xml
└── src
    └── xela_taxel_joint_state_publisher.cpp

1. Overview¶

xela_taxel_joint_state_publisher is a ros2_control controller that publishes a filtered JointState stream for XELA taxel sensors. The controller filters incoming JointState messages or initializes joint positions from configured lists, then publishes a consolidated JointState topic for downstream applications. It is designed to run inside ros2_control and be managed by controller_manager.

2. Goals¶

- Provide a ros2_control controller that publishes taxel JointState for a selected device profile.
- Support multiple device profiles (e.g., allegrohand, 2f_gripper) from a single YAML config.
- Allow configuration of output topic, publish rate, input sources, and joint ordering.
- Operate without impacting the primary MoveIt control JointState stream.
- Be robust to missing inputs by using initial positions where available.

3. Non-Goals¶

- Parsing URDFs or XACROs directly (handled by upstream nodes or config files).
- Dynamically reloading device profiles at runtime (restart/reload expected).
- Publishing non-joint sensor data (force, tactile values, etc.).

4. Users and Use Cases¶

4.1 Users¶

- Robotics developers integrating XELA taxel sensors into MoveIt Pro configurations.
- Operators running robots that require a separate full-model joint state stream.

4.2 Use Cases¶

- Publish taxel joint states for a full robot model on /joint_states_full while keeping /joint_states lightweight for MoveIt.
- Publish taxel joint states for a 2-finger gripper package on /joint_states.
- Combine multiple taxel device profiles into one output stream.

5. Functional Requirements¶

5.1 Configuration¶

- Controller must read a YAML config file specified by config_yaml.
- Config supports device_profiles with keep_joints_files lists per profile.
- Config supports optional global parameters: output_topic, publish_rate, preserve_input_order, source_list.
- Controller accepts parameter device_profiles to select one or more profiles.

5.2 Input JointState Handling¶

- Controller subscribes to topics listed in source_list.
- If source_list is empty, the controller publishes using initial positions only.
- If a source topic equals output_topic, it is ignored to prevent loops.

5.3 Output JointState¶

- Output topic is output_topic (default /joint_states).
- Output includes name and position arrays. velocity and effort are only included if present in inputs.
- Output ordering:
- If ordered_keep_joints exists in config, it defines output order.
- Else if preserve_input_order is true and input received, use input order filtered to keep joints.
- Else output order follows merged keep_joints list.

5.4 Initial Positions¶

- Controller supports initial_positions per profile in config (optional).
- If no position has been received for a joint, initial position is used.

5.5 Package Path Resolution¶

- config_yaml supports package://<pkg>/path resolution using ament index.

6. Non-Functional Requirements¶

- ROS 2 Humble compatible.
- Real-time safe behavior: no blocking operations in update().
- Robust to missing inputs and config errors (log and fail gracefully on configure).
- Maintain deterministic output order per configuration.

7. Configuration Schema¶

7.1 Example¶

output_topic: /joint_states
publish_rate: 30.0
preserve_input_order: true
source_list:
  - /joint_states_full

device_profiles:
  allegrohand:
    keep_joints_files:
      - list_allegrohand_joint.yaml
  2f_gripper:
    keep_joints_files:
      - list_2f_gripper_joint.yaml

7.2 keep_joints_files¶

- Each file is YAML containing keep_joints: followed by a list of joint names.

8. Interface (Parameters)¶

9. Integration Requirements¶

- Controller must be listed under controller_manager in the target ros2_control YAML.
- Controller parameters must be defined under a top-level node matching controller name.
- Add controller to controllers_active_at_startup in MoveIt Pro config if required.

10. Failure Modes and Logging¶

- If config_yaml is missing or invalid, controller configuration fails.
- If device_profiles is empty or missing in config, configuration fails.
- If keep_joints resolves to empty, configuration fails.
- All failures must log clear error messages.

11. Testing Plan¶

- Unit-style validation via:
- Launch controller with valid config and verify output topic publishes expected joints.
- Launch controller without source_list and validate initial positions publish.
- Launch controller with invalid config and ensure configure fails gracefully.
- Integration validation with MoveIt Pro configuration:
- Confirm controller is listed by ros2 control list_controllers.
- Confirm output topic is populated with expected taxel joints.

12. Open Questions¶

- Should source_list allow wildcard or namespace expansion?
- Should publish be suppressed until at least one input message is received?
- Do we need runtime profile switching without controller restart?

config/control/ur5e_x2f_85.ros2_control.yaml ShowHide

controller_manager:
  ros__parameters:
    update_rate: 600  # Hz
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    io_and_status_controller:
      type: ur_controllers/GPIOController
    force_torque_sensor_broadcaster:
      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    robotiq_gripper_controller:
      type: position_controllers/GripperActionController
    robotiq_activation_controller:
      type: robotiq_controllers/RobotiqActivationController
    servo_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    joint_trajectory_admittance_controller:
      type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
    velocity_force_controller:
      type: velocity_force_controller/VelocityForceController
    xela_taxel_joint_state_publisher:
      type: xela_taxel_joint_state_publisher/XelaTaxelJointStatePublisher

io_and_status_controller:
  ros__parameters:
    tf_prefix: "" 

force_torque_sensor_broadcaster:
  ros__parameters:
    sensor_name: tcp_fts_sensor
    state_interface_names:
      - force.x
      - force.y
      - force.z
      - torque.x
      - torque.y
      - torque.z
    frame_id: tool0

joint_trajectory_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    command_joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.0
      goal_time: 0.0
      shoulder_pan_joint:
        goal: 0.05
      shoulder_lift_joint:
        goal: 0.05
      elbow_joint:
        goal: 0.05
      wrist_1_joint:
        goal: 0.05
      wrist_2_joint:
        goal: 0.05
      wrist_3_joint:
        goal: 0.05

servo_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    command_joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.0
      goal_time: 0.0
      shoulder_pan_joint:
        goal: 0.05
      shoulder_lift_joint:
        goal: 0.05
      elbow_joint:
        goal: 0.05
      wrist_1_joint:
        goal: 0.05
      wrist_2_joint:
        goal: 0.05
      wrist_3_joint:
        goal: 0.05

robotiq_gripper_controller:
  ros__parameters:
    default: true
    joint: robotiq_85_left_knuckle_joint
    allow_stalling: true
    # These need to be tuned for HW:
    # stall_timeout: 0.05
    # goal_tolerance: 0.02

robotiq_activation_controller:
  ros__parameters:
    default: true

joint_trajectory_admittance_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    base_frame: base_link
    sensor_frame: tool0
    ee_frame: grasp_link
    ft_sensor_name: tcp_fts_sensor
    # Joint accelerations chosen to match MoveIt configs.
    stop_accelerations:
      - 30.0
      - 30.0
      - 30.0
      - 30.0
      - 30.0
      - 30.0

velocity_force_controller:
  ros__parameters:
    planning_group_name: manipulator
    sensor_frame: tool0
    ee_frame: grasp_link
    ft_sensor_name: tcp_fts_sensor
    ft_force_deadband: 2.0
    ft_torque_deadband: 1.0
    max_joint_velocity:
      - 0.524
      - 0.524
      - 0.524
      - 1.047
      - 1.047
      - 1.047
    max_joint_acceleration:
      - 52.4
      - 52.4
      - 52.4
      - 52.4
      - 52.4
      - 52.4
    max_cartesian_velocity:
      - 0.25
      - 0.25
      - 0.25
      - 1.5707
      - 1.5707
      - 1.5707
    max_cartesian_acceleration:
      - 20.0
      - 20.0
      - 20.0
      - 40.0
      - 40.0
      - 40.0

xela_taxel_joint_state_publisher:
  ros__parameters:
    config_yaml: package://xela_taxel_joint_state_publisher/config/xela_taxel_jsp_config.yaml
    device_profiles:
      - 2f_gripper
    output_topic: /joint_states
    # source_list (optional): list of input joint_state topics to merge; omit to use defaults.
    publish_rate: 30.0
    preserve_input_order: true

config/config.yaml ShowHide

based_on_package: "ur5e_x2f_85_common" 

hardware:
  simulated: true
  robot_description:
    urdf:
      package: "ur5e_x2f_85_config1" 
      path: "description/ur5e_x2f_85.xacro" 
    srdf:
      package: "ur5e_x2f_85_config1" 
      path: "config/moveit/ur5e_x2f_85.srdf" 
    urdf_params:
      - name: "ur5e_x2f_85" 
      - prefix: "" 
      - use_fake_hardware: "true" 
      - use_pinch_links: "true" 
      - mock_sensor_commands: "false" 
      - headless_mode: "true" 
      - robot_ip: "0.0.0.0" 
      - has_tool_changer: "true" 
      - xela_taxels: "1" 
      - robotiq_gripper_closed_position: "0.65" 
      - robotiq_finger_joint_initial_value: "0.65" 
      - joint_limits_parameters_file:
          package: "ur5e_x2f_85_config1" 
          path: "config/moveit/joint_limits.yaml" 
      - kinematics_parameters_file:
          package: "ur_description" 
          path: "config/ur5e/default_kinematics.yaml" 
      - physical_parameters_file:
          package: "ur_description" 
          path: "config/ur5e/physical_parameters.yaml" 
      - visual_parameters_file:
          package: "ur_description" 
          path: "config/ur5e/visual_parameters.yaml" 

moveit_params:
  joint_group_name: "manipulator" 
  joint_limits:
    package: "ur5e_x2f_85_config1" 
    path: "config/moveit/joint_limits.yaml" 

objectives:
  objective_library_paths:
    common_objectives:
      package_name: "ur5e_x2f_85_common" 
      relative_path: "objectives" 
    custom_objectives:
      package_name: "ur5e_x2f_85_config1" 
      relative_path: "objectives" 

ros2_control:
  config:
    package: "ur5e_x2f_85_config1" 
    path: "config/control/ur5e_x2f_85.ros2_control.yaml" 
  controllers_active_at_startup:
    - "force_torque_sensor_broadcaster" 
    - "robotiq_gripper_controller" 
    - "joint_state_broadcaster" 
    - "xela_taxel_joint_state_publisher" 
    - "servo_controller" 
    - "io_and_status_controller" 
    - "robotiq_activation_controller" 
  controllers_inactive_at_startup:
    - "joint_trajectory_controller" 
    - "joint_trajectory_admittance_controller" 
    - "velocity_force_controller" 
  controllers_not_managed: []
  controller_shared_topics: []

invokelee@🐙MoveIt Pro🐙:~/user_ws$ ros2 control list_controllers
force_torque_sensor_broadcaster        force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster                active
velocity_force_controller              velocity_force_controller/VelocityForceController                           inactive
robotiq_activation_controller          robotiq_controllers/RobotiqActivationController                             active
xela_taxel_joint_state_publisher       xela_taxel_joint_state_publisher/XelaTaxelJointStatePublisher               active
joint_state_broadcaster                joint_state_broadcaster/JointStateBroadcaster                               active
robotiq_gripper_controller             position_controllers/GripperActionController                                active
joint_trajectory_admittance_controller joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController  inactive
servo_controller                       joint_trajectory_controller/JointTrajectoryController                       inactive
joint_trajectory_controller            joint_trajectory_controller/JointTrajectoryController                       active

invokelee@🐙MoveIt Pro🐙:~/user_ws$ ros2 topic info /joint_states -v
Type: sensor_msgs/msg/JointState

Publisher count: 2

Node name: xela_taxel_joint_state_publisher
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: PUBLISHER
GID: 01.10.e6.cb.8e.80.fa.90.a8.dc.e6.8f.00.00.ef.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite