feat: state_broadcaster implementation

CMakeLists.txt

@@ -61,6 +61,8 @@ find_package(Eigen3 REQUIRED)
 find_package(pinocchio REQUIRED)
 find_package(realtime_tools REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 
 generate_parameter_library(
@@ -69,20 +71,15 @@ generate_parameter_library(
 )
 
 generate_parameter_library(
-  pose_broadcaster_parameters
-  src/pose_broadcaster.yaml
+  state_broadcaster_parameters
+  src/state_broadcaster.yaml
 )
 
 generate_parameter_library(
   torque_feedback_controller_parameters
   src/torque_feedback_controller.yaml
 )
 
-generate_parameter_library(
-  twist_broadcaster_parameters
-  src/twist_broadcaster.yaml
-)
-
 generate_parameter_library(
   cartesian_admittance_controller_parameters
   src/cartesian_admittance_controller.yaml
@@ -93,9 +90,8 @@ add_library(
         SHARED
         src/cartesian_controller.cpp
         src/cartesian_admittance_controller.cpp
-        src/pose_broadcaster.cpp
+        src/state_broadcaster.cpp
         src/torque_feedback_controller.cpp
-        src/twist_broadcaster.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -121,9 +117,8 @@ target_link_libraries(${PROJECT_NAME}
   PRIVATE
     cartesian_controller_parameters
     cartesian_admittance_controller_parameters
-    pose_broadcaster_parameters
+    state_broadcaster_parameters
     torque_feedback_controller_parameters
-    twist_broadcaster_parameters
 )
 
 
@@ -138,6 +133,8 @@ ament_target_dependencies(${PROJECT_NAME}
     generate_parameter_library
     realtime_tools
     std_msgs
+    geometry_msgs
+    sensor_msgs
 )
 
 pluginlib_export_plugin_description_file(
@@ -166,6 +163,8 @@ ament_export_dependencies(
     pinocchio
     realtime_tools
     std_msgs
+    geometry_msgs
+    sensor_msgs
     generate_parameter_library
 )
 

crisp_controllers.xml

@@ -11,22 +11,28 @@
             Cartesian admittance controller with impedance outer loop for compliant force-based interaction.
         </description>
     </class>
-    <class name="crisp_controllers/PoseBroadcaster"
-           type="crisp_controllers::PoseBroadcaster" base_class_type="controller_interface::ControllerInterface">
+    <class name="crisp_controllers/StateBroadcaster"
+           type="crisp_controllers::StateBroadcaster" base_class_type="controller_interface::ControllerInterface">
         <description>
-            Simple broadcaster for the pose.
+            Broadcaster for joint, Cartesian pose, twist, and wrench state outputs.
         </description>
     </class>
-    <class name="crisp_controllers/TorqueFeedbackController"
-           type="crisp_controllers::TorqueFeedbackController" base_class_type="controller_interface::ControllerInterface">
+    <class name="crisp_controllers/PoseBroadcaster"
+           type="crisp_controllers::StateBroadcaster" base_class_type="controller_interface::ControllerInterface">
         <description>
-            Torque feedback controller for robotic manipulators that responds to external torques with PD control and friction compensation.
+            Backwards-compatible alias for StateBroadcaster.
         </description>
     </class>
     <class name="crisp_controllers/TwistBroadcaster"
-           type="crisp_controllers::TwistBroadcaster" base_class_type="controller_interface::ControllerInterface">
+           type="crisp_controllers::StateBroadcaster" base_class_type="controller_interface::ControllerInterface">
         <description>
-            Simple broadcaster for the twist velocity.
+            Backwards-compatible alias for StateBroadcaster.
+        </description>
+    </class>
+    <class name="crisp_controllers/TorqueFeedbackController"
+           type="crisp_controllers::TorqueFeedbackController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Torque feedback controller for robotic manipulators that responds to external torques with PD control and friction compensation.
         </description>
     </class>
 </library>

docs/new_robot_setup.md

@@ -47,8 +47,8 @@ For more information on the controllers, check the [controllers details](getting
             ros__parameters:
             update_rate: 1000  # Hz
 
-            pose_broadcaster:
-                type: crisp_controllers/PoseBroadcaster
+            state_broadcaster:
+                type: crisp_controllers/StateBroadcaster
 
             gravity_compensation:
                 type: crisp_controllers/CartesianController
@@ -61,14 +61,22 @@ For more information on the controllers, check the [controllers details](getting
 
             # more controllers...
         /**:
-        pose_broadcaster:
+        state_broadcaster:
             ros__parameters:
             joints:
                 - <TODO>
 
             end_effector_frame: <TODO>
             base_frame: base
 
+            pose:
+                topic: current_pose
+                publish_frequency: 500.0
+
+            twist:
+                topic: current_twist
+                publish_frequency: 500.0
+
         gravity_compensation:
             ros__parameters:
             joints:
@@ -182,12 +190,28 @@ For more information on the controllers, check the [controllers details](getting
         Node(
             package="controller_manager",
             executable="spawner",
-            arguments=["pose_broadcaster"],
+            arguments=["state_broadcaster"],
             output="screen",
         ),
         ...
         ```
 
+    !!! note "Migrating old pose/twist broadcaster parameters"
+        `crisp_controllers/StateBroadcaster` replaces the old pose and twist broadcaster setup. If an old config used flat `topic` or `publish_frequency` parameters, move them under the matching output:
+
+        ```yaml
+        state_broadcaster:
+            ros__parameters:
+                pose:
+                    topic: current_pose
+                    publish_frequency: 500.0
+                twist:
+                    topic: current_twist
+                    publish_frequency: 500.0
+        ```
+
+        Old pose broadcaster `topic` and `publish_frequency` values become `pose.topic` and `pose.publish_frequency`. Old twist broadcaster values become `twist.topic` and `twist.publish_frequency`. If both old broadcasters existed, merge both nested blocks into the single `state_broadcaster` config.
+
 6. After launching your robot you should see that new controller are being loaded. If you get stuck somewhere in the process feel free to open an issue.
 7. Finally, to use the robots in crisp_py, add a configuration file for the new robot and Gymnasium environments that use it.
 
@@ -234,4 +258,3 @@ For more information on the controllers, check the [controllers details](getting
         In a similar manner, you can add this config to an Gymnasium environment to create a Gymnasium env with this config!
 
 8. Voila, you are good to go!
-