joint_state_broadcaster to use realtime tools (#276)

* Use RealtimePublisher for joint_states

* Use RealtimePublisher for dynamic joint states

Author: bmagyar

joint_state_broadcaster/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(controller_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils REQUIRED)
+find_package(realtime_tools REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
 add_library(joint_state_broadcaster
@@ -30,6 +31,7 @@ ament_target_dependencies(joint_state_broadcaster
   pluginlib
   rclcpp_lifecycle
   rcutils
+  realtime_tools
   sensor_msgs
 )
 # Causes the visibility macros to use dllexport rather than dllimport,

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -25,6 +25,7 @@
 #include "joint_state_broadcaster/visibility_control.h"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "realtime_tools/realtime_publisher.h"
 #include "sensor_msgs/msg/joint_state.hpp"
 
 namespace joint_state_broadcaster
@@ -100,14 +101,16 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   //  we store the name of joints with compatible interfaces
   std::vector<std::string> joint_names_;
   std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> joint_state_publisher_;
-  sensor_msgs::msg::JointState joint_state_msg_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>
+    realtime_joint_state_publisher_;
 
   //  For the DynamicJointState format, we use a map to buffer values in for easier lookup
   //  This allows to preserve whatever order or names/interfaces were initialized.
   std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
   std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
     dynamic_joint_state_publisher_;
-  control_msgs::msg::DynamicJointState dynamic_joint_state_msg_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
+    realtime_dynamic_joint_state_publisher_;
 };
 
 }  // namespace joint_state_broadcaster

joint_state_broadcaster/package.xml

@@ -23,6 +23,7 @@
   <depend>hardware_interface</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
+  <depend>realtime_tools</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -156,9 +156,17 @@ CallbackReturn JointStateBroadcaster::on_configure(
     joint_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
       topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS());
 
+    realtime_joint_state_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(
+        joint_state_publisher_);
+
     dynamic_joint_state_publisher_ =
       get_node()->create_publisher<control_msgs::msg::DynamicJointState>(
         topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
+
+    realtime_dynamic_joint_state_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(
+        dynamic_joint_state_publisher_);
   }
   catch (const std::exception & e)
   {
@@ -281,26 +289,28 @@ void JointStateBroadcaster::init_joint_state_msg()
   /// with at least one of these interfaces, the rest are omitted from this message
 
   // default initialization for joint state message
-  joint_state_msg_.name = joint_names_;
-  joint_state_msg_.position.resize(num_joints, kUninitializedValue);
-  joint_state_msg_.velocity.resize(num_joints, kUninitializedValue);
-  joint_state_msg_.effort.resize(num_joints, kUninitializedValue);
+  auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
+  joint_state_msg.name = joint_names_;
+  joint_state_msg.position.resize(num_joints, kUninitializedValue);
+  joint_state_msg.velocity.resize(num_joints, kUninitializedValue);
+  joint_state_msg.effort.resize(num_joints, kUninitializedValue);
 }
 
 void JointStateBroadcaster::init_dynamic_joint_state_msg()
 {
+  auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
   for (const auto & name_ifv : name_if_value_mapping_)
   {
     const auto & name = name_ifv.first;
     const auto & interfaces_and_values = name_ifv.second;
-    dynamic_joint_state_msg_.joint_names.push_back(name);
+    dynamic_joint_state_msg.joint_names.push_back(name);
     control_msgs::msg::InterfaceValue if_value;
     for (const auto & interface_and_value : interfaces_and_values)
     {
       if_value.interface_names.emplace_back(interface_and_value.first);
       if_value.values.emplace_back(kUninitializedValue);
     }
-    dynamic_joint_state_msg_.interface_values.emplace_back(if_value);
+    dynamic_joint_state_msg.interface_values.emplace_back(if_value);
   }
 }
 
@@ -342,37 +352,45 @@ controller_interface::return_type JointStateBroadcaster::update(
       state_interface.get_value());
   }
 
-  joint_state_msg_.header.stamp = time;
-  dynamic_joint_state_msg_.header.stamp = time;
-
-  // update joint state message and dynamic joint state message
-  for (size_t i = 0; i < joint_names_.size(); ++i)
+  if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock())
   {
-    joint_state_msg_.position[i] =
-      get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION);
-    joint_state_msg_.velocity[i] =
-      get_value(name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY);
-    joint_state_msg_.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT);
+    auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
+
+    joint_state_msg.header.stamp = time;
+
+    // update joint state message and dynamic joint state message
+    for (size_t i = 0; i < joint_names_.size(); ++i)
+    {
+      joint_state_msg.position[i] =
+        get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION);
+      joint_state_msg.velocity[i] =
+        get_value(name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY);
+      joint_state_msg.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT);
+    }
+    realtime_joint_state_publisher_->unlockAndPublish();
   }
 
-  for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg_.joint_names.size();
-       ++joint_index)
+  if (realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock())
   {
-    const auto & name = dynamic_joint_state_msg_.joint_names[joint_index];
-    for (size_t interface_index = 0;
-         interface_index <
-         dynamic_joint_state_msg_.interface_values[joint_index].interface_names.size();
-         ++interface_index)
+    auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
+    dynamic_joint_state_msg.header.stamp = time;
+    for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg.joint_names.size();
+         ++joint_index)
     {
-      const auto & interface_name =
-        dynamic_joint_state_msg_.interface_values[joint_index].interface_names[interface_index];
-      dynamic_joint_state_msg_.interface_values[joint_index].values[interface_index] =
-        name_if_value_mapping_[name][interface_name];
+      const auto & name = dynamic_joint_state_msg.joint_names[joint_index];
+      for (size_t interface_index = 0;
+           interface_index <
+           dynamic_joint_state_msg.interface_values[joint_index].interface_names.size();
+           ++interface_index)
+      {
+        const auto & interface_name =
+          dynamic_joint_state_msg.interface_values[joint_index].interface_names[interface_index];
+        dynamic_joint_state_msg.interface_values[joint_index].values[interface_index] =
+          name_if_value_mapping_[name][interface_name];
+      }
     }
+    realtime_dynamic_joint_state_publisher_->unlockAndPublish();
   }
-  // publish
-  joint_state_publisher_->publish(joint_state_msg_);
-  dynamic_joint_state_publisher_->publish(dynamic_joint_state_msg_);
 
   return controller_interface::return_type::OK;
 }