add external force broadcasters

ICube-Robotics · Nov 8, 2023 · 13206aa · 13206aa
1 parent 887c030
commit 13206aa
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 0 deletions.
diff --git a/hk1d_bringup_master_follower/config/hk1d_controllers.yaml b/hk1d_bringup_master_follower/config/hk1d_controllers.yaml
@@ -4,16 +4,50 @@ controller_manager:
 
     control_freq: 200
 
+    # Broadcasters
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    master_force_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    follower_force_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    # Controllers
     forward_effort_controller:
       type: forward_command_controller/ForwardCommandController
 
+    # Mock components
+    mock_follower_force_sensor:
+      type: forward_command_controller/ForwardCommandController
+
+# Broadcasters
+master_force_sensor_broadcaster:
+  ros__parameters:
+    frame_id: master_ft_sensor
+    interface_names:
+      force:
+        x: master_ft_sensor/force.0
+
+follower_force_sensor_broadcaster:
+  ros__parameters:
+    frame_id: follower_ft_sensor
+    interface_names:
+      force:
+        x: follower_ft_sensor/force.0
 
+# Controllers
 forward_position_controller:
   ros__parameters:
     joints:
       - master_joint_1
       - follower_joint_1
     interface_name: effort
+
+# Mock components
+mock_follower_force_sensor:
+  ros__parameters:
+    joints:
+      - master_force_sensor
+    interface_name: force.0
diff --git a/hk1d_bringup_master_follower/launch/hk1d.launch.py b/hk1d_bringup_master_follower/launch/hk1d.launch.py
@@ -171,10 +171,23 @@ def get_robot_description_semantic_content():
         arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
     )
 
+    master_force_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["master_force_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+    follower_force_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["follower_force_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
     nodes = [
         control_node,
         robot_state_pub_node,
         joint_state_broadcaster_spawner,
+        master_force_sensor_broadcaster_spawner,
+        follower_force_sensor_broadcaster_spawner,
         rviz_node,
     ]