feat: state_broadcaster implementation

Author: dmalexa5

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,10 +11,10 @@
             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"
@@ -23,10 +23,4 @@
             Torque feedback controller for robotic manipulators that responds to external torques with PD control and friction compensation.
         </description>
     </class>
-    <class name="crisp_controllers/TwistBroadcaster"
-           type="crisp_controllers::TwistBroadcaster" base_class_type="controller_interface::ControllerInterface">
-        <description>
-            Simple broadcaster for the twist velocity.
-        </description>
-    </class>
 </library>

include/crisp_controllers/cartesian_controller.hpp

@@ -18,6 +18,7 @@
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
 
 #include <crisp_controllers/utils/ros2_version.hpp>
 
@@ -106,6 +107,14 @@ class CartesianController : public controller_interface::ControllerInterface {
   /** @brief Subscription for variable stiffness messages */
   rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr stiffness_sub_;
 
+  /** @brief Publisher for commanded joint effort values */
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr effort_publisher_;
+  /** @brief Realtime publisher for commanded joint effort values */
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>
+    realtime_effort_publisher_;
+  /** @brief Pre-sized commanded effort message for realtime publication */
+  std_msgs::msg::Float64MultiArray effort_msg_;
+
   /** @brief Flag to indicate if multiple publishers detected */
   bool multiple_publishers_detected_;
 
@@ -274,6 +283,11 @@ class CartesianController : public controller_interface::ControllerInterface {
   /** @brief Final desired torque command */
   Eigen::VectorXd tau_d;
 
+  /** @brief Time accumulated since the last effort publication */
+  rclcpp::Duration publish_elapsed_{0, 0};
+  /** @brief Effort publication interval */
+  rclcpp::Duration publish_interval_{0, 0};
+
   /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */
   Eigen::Matrix<double, 6, 6> Mx_inv = Eigen::Matrix<double, 6, 6>::Zero();
   /** @brief the manipulator joint mass projected in Cartesian space (6x6) */