Update API for realtime publisher

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -130,6 +130,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
     input_joint_command_;
   realtime_tools::RealtimeThreadSafeBox<geometry_msgs::msg::WrenchStamped> input_wrench_command_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
+  ControllerStateMsg state_msg_;
 
   // save the last commands in case of unable to get value from box
   trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_;

admittance_controller/src/admittance_controller.cpp

@@ -336,9 +336,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
     std::make_unique<realtime_tools::RealtimePublisher<ControllerStateMsg>>(s_publisher_);
 
   // Initialize state message
-  state_publisher_->lock();
-  state_publisher_->msg_ = admittance_->get_controller_state();
-  state_publisher_->unlock();
+  state_msg_ = admittance_->get_controller_state();
 
   // Initialize FTS semantic semantic_component
   force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(
@@ -492,9 +490,11 @@ controller_interface::return_type AdmittanceController::update_and_write_command
   write_state_to_hardware(reference_admittance_);
 
   // Publish controller state
-  state_publisher_->lock();
-  state_publisher_->msg_ = admittance_->get_controller_state();
-  state_publisher_->unlockAndPublish();
+  if (state_publisher_)
+  {
+    state_msg_ = admittance_->get_controller_state();
+    state_publisher_->try_publish(state_msg_);
+  }
 
   return controller_interface::return_type::OK;
 }

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -113,11 +113,13 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
     realtime_odometry_publisher_ = nullptr;
+  nav_msgs::msg::Odometry odometry_message_;
 
   std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
     nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
     realtime_odometry_transform_publisher_ = nullptr;
+  tf2_msgs::msg::TFMessage odometry_transform_message_;
 
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
@@ -136,6 +138,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
     realtime_limited_velocity_publisher_ = nullptr;
+  TwistStamped limited_velocity_message_;
 
   rclcpp::Time previous_update_timestamp_{0};
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -231,32 +231,31 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
 
   if (should_publish)
   {
-    if (realtime_odometry_publisher_->trylock())
+    if (realtime_odometry_publisher_)
     {
-      auto & odometry_message = realtime_odometry_publisher_->msg_;
-      odometry_message.header.stamp = time;
-      odometry_message.pose.pose.position.x = odometry_.getX();
-      odometry_message.pose.pose.position.y = odometry_.getY();
-      odometry_message.pose.pose.orientation.x = orientation.x();
-      odometry_message.pose.pose.orientation.y = orientation.y();
-      odometry_message.pose.pose.orientation.z = orientation.z();
-      odometry_message.pose.pose.orientation.w = orientation.w();
-      odometry_message.twist.twist.linear.x = odometry_.getLinear();
-      odometry_message.twist.twist.angular.z = odometry_.getAngular();
-      realtime_odometry_publisher_->unlockAndPublish();
+      odometry_message_.header.stamp = time;
+      odometry_message_.pose.pose.position.x = odometry_.getX();
+      odometry_message_.pose.pose.position.y = odometry_.getY();
+      odometry_message_.pose.pose.orientation.x = orientation.x();
+      odometry_message_.pose.pose.orientation.y = orientation.y();
+      odometry_message_.pose.pose.orientation.z = orientation.z();
+      odometry_message_.pose.pose.orientation.w = orientation.w();
+      odometry_message_.twist.twist.linear.x = odometry_.getLinear();
+      odometry_message_.twist.twist.angular.z = odometry_.getAngular();
+      realtime_odometry_publisher_->try_publish(odometry_message_);
     }
 
-    if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
+    if (params_.enable_odom_tf && realtime_odometry_transform_publisher_)
     {
-      auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
+      auto & transform = odometry_transform_message_.transforms.front();
       transform.header.stamp = time;
       transform.transform.translation.x = odometry_.getX();
       transform.transform.translation.y = odometry_.getY();
       transform.transform.rotation.x = orientation.x();
       transform.transform.rotation.y = orientation.y();
       transform.transform.rotation.z = orientation.z();
       transform.transform.rotation.w = orientation.w();
-      realtime_odometry_transform_publisher_->unlockAndPublish();
+      realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_);
     }
   }
 
@@ -271,17 +270,16 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   previous_two_commands_.push({{linear_command, angular_command}});
 
   //    Publish limited velocity
-  if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock())
-  {
-    auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
-    limited_velocity_command.header.stamp = time;
-    limited_velocity_command.twist.linear.x = linear_command;
-    limited_velocity_command.twist.linear.y = 0.0;
-    limited_velocity_command.twist.linear.z = 0.0;
-    limited_velocity_command.twist.angular.x = 0.0;
-    limited_velocity_command.twist.angular.y = 0.0;
-    limited_velocity_command.twist.angular.z = angular_command;
-    realtime_limited_velocity_publisher_->unlockAndPublish();
+  if (publish_limited_velocity_ && realtime_limited_velocity_publisher_)
+  {
+    limited_velocity_message_.header.stamp = time;
+    limited_velocity_message_.twist.linear.x = linear_command;
+    limited_velocity_message_.twist.linear.y = 0.0;
+    limited_velocity_message_.twist.linear.z = 0.0;
+    limited_velocity_message_.twist.angular.x = 0.0;
+    limited_velocity_message_.twist.angular.y = 0.0;
+    limited_velocity_message_.twist.angular.z = angular_command;
+    realtime_limited_velocity_publisher_->try_publish(limited_velocity_message_);
   }
 
   // Compute wheels velocities:
@@ -469,10 +467,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
       odometry_transform_publisher_);
 
   // keeping track of odom and base_link transforms only
-  auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
-  odometry_transform_message.transforms.resize(1);
-  odometry_transform_message.transforms.front().header.frame_id = odom_frame_id;
-  odometry_transform_message.transforms.front().child_frame_id = base_frame_id;
+  odometry_transform_message_.transforms.resize(1);
+  odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id;
+  odometry_transform_message_.transforms.front().child_frame_id = base_frame_id;
 
   previous_update_timestamp_ = get_node()->get_clock()->now();
   return controller_interface::CallbackReturn::SUCCESS;

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -71,6 +71,7 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr
   using StatePublisher = realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>;
   rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr sensor_state_publisher_;
   std::unique_ptr<StatePublisher> realtime_publisher_;
+  geometry_msgs::msg::WrenchStamped sensor_state_message_;
 };
 
 }  // namespace force_torque_sensor_broadcaster

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -102,9 +102,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  realtime_publisher_->lock();
-  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
-  realtime_publisher_->unlock();
+  sensor_state_message_.header.frame_id = params_.frame_id;
 
   RCLCPP_INFO(get_node()->get_logger(), "configure successful");
   return controller_interface::CallbackReturn::SUCCESS;
@@ -146,13 +144,13 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write
 {
   param_listener_->try_get_params(params_);
 
-  if (realtime_publisher_ && realtime_publisher_->trylock())
+  if (realtime_publisher_)
   {
-    realtime_publisher_->msg_.header.stamp = time;
-    force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
-    this->apply_sensor_offset(params_, realtime_publisher_->msg_);
-    this->apply_sensor_multiplier(params_, realtime_publisher_->msg_);
-    realtime_publisher_->unlockAndPublish();
+    sensor_state_message_.header.stamp = time;
+    force_torque_sensor_->get_values_as_message(sensor_state_message_.wrench);
+    this->apply_sensor_offset(params_, sensor_state_message_);
+    this->apply_sensor_multiplier(params_, sensor_state_message_);
+    realtime_publisher_->try_publish(sensor_state_message_);
   }
 
   return controller_interface::return_type::OK;

gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp

@@ -100,6 +100,7 @@ class GpioCommandController : public controller_interface::ControllerInterface
 
   std::shared_ptr<rclcpp::Publisher<StateType>> gpio_state_publisher_{};
   std::shared_ptr<realtime_tools::RealtimePublisher<StateType>> realtime_gpio_state_publisher_{};
+  StateType gpio_state_msg_;
 
   std::shared_ptr<gpio_command_controller_parameters::ParamListener> param_listener_{};
   gpio_command_controller_parameters::Params params_;

gpio_controllers/src/gpio_command_controller.cpp

@@ -252,19 +252,18 @@ void GpioCommandController::store_state_interface_types()
 
 void GpioCommandController::initialize_gpio_state_msg()
 {
-  auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_;
-  gpio_state_msg.header.stamp = get_node()->now();
-  gpio_state_msg.interface_groups.resize(params_.gpios.size());
-  gpio_state_msg.interface_values.resize(params_.gpios.size());
+  gpio_state_msg_.header.stamp = get_node()->now();
+  gpio_state_msg_.interface_groups.resize(params_.gpios.size());
+  gpio_state_msg_.interface_values.resize(params_.gpios.size());
 
   for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index)
   {
     const auto gpio_name = params_.gpios[gpio_index];
-    gpio_state_msg.interface_groups[gpio_index] = gpio_name;
-    gpio_state_msg.interface_values[gpio_index].interface_names =
+    gpio_state_msg_.interface_groups[gpio_index] = gpio_name;
+    gpio_state_msg_.interface_values[gpio_index].interface_names =
       get_gpios_state_interfaces_names(gpio_name);
-    gpio_state_msg.interface_values[gpio_index].values = std::vector<double>(
-      gpio_state_msg.interface_values[gpio_index].interface_names.size(),
+    gpio_state_msg_.interface_values[gpio_index].values = std::vector<double>(
+      gpio_state_msg_.interface_values[gpio_index].interface_names.size(),
       std::numeric_limits<double>::quiet_NaN());
   }
 }
@@ -391,24 +390,23 @@ void GpioCommandController::apply_command(
 
 void GpioCommandController::update_gpios_states()
 {
-  if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock())
+  if (!realtime_gpio_state_publisher_)
   {
     return;
   }
 
-  auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_;
-  gpio_state_msg.header.stamp = get_node()->now();
-  for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size();
+  gpio_state_msg_.header.stamp = get_node()->now();
+  for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg_.interface_groups.size();
        ++gpio_index)
   {
     for (std::size_t interface_index = 0;
-         interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size();
+         interface_index < gpio_state_msg_.interface_values[gpio_index].interface_names.size();
          ++interface_index)
     {
-      apply_state_value(gpio_state_msg, gpio_index, interface_index);
+      apply_state_value(gpio_state_msg_, gpio_index, interface_index);
     }
   }
-  realtime_gpio_state_publisher_->unlockAndPublish();
+  realtime_gpio_state_publisher_->try_publish(gpio_state_msg_);
 }
 
 void GpioCommandController::apply_state_value(

gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp

@@ -57,8 +57,11 @@ class GPSSensorBroadcaster : public controller_interface::ControllerInterface
   callback_return_type setup_publisher();
 
   GPSSensorVariant gps_sensor_;
+
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr sensor_state_publisher_;
   std::unique_ptr<StatePublisher> realtime_publisher_;
+  sensor_msgs::msg::NavSatFix state_message_;
+
   std::shared_ptr<gps_sensor_broadcaster::ParamListener> param_listener_{};
   gps_sensor_broadcaster::Params params_;
   std::vector<std::string> state_names_;

gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp

@@ -78,10 +78,8 @@ callback_return_type GPSSensorBroadcaster::setup_publisher()
     sensor_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::NavSatFix>(
       "~/gps/fix", rclcpp::SystemDefaultsQoS());
     realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
-    realtime_publisher_->lock();
-    realtime_publisher_->msg_.header.frame_id = params_.frame_id;
+    state_message_.header.frame_id = params_.frame_id;
     setup_covariance();
-    realtime_publisher_->unlock();
 
     return callback_return_type::SUCCESS;
   }
@@ -98,14 +96,14 @@ void GPSSensorBroadcaster::setup_covariance()
 {
   if (params_.read_covariance_from_interface)
   {
-    realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN;
+    state_message_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN;
     return;
   }
 
   for (size_t i = 0; i < COVARIANCE_3x3_SIZE; ++i)
   {
-    realtime_publisher_->msg_.position_covariance[i] = params_.static_position_covariance[i];
-    realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_KNOWN;
+    state_message_.position_covariance[i] = params_.static_position_covariance[i];
+    state_message_.position_covariance_type = COVARIANCE_TYPE_KNOWN;
   }
 }
 
@@ -148,19 +146,19 @@ callback_return_type GPSSensorBroadcaster::on_deactivate(const rclcpp_lifecycle:
 controller_interface::return_type GPSSensorBroadcaster::update(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
-  if (realtime_publisher_ && realtime_publisher_->trylock())
+  if (realtime_publisher_)
   {
-    realtime_publisher_->msg_.header.stamp = get_node()->now();
+    state_message_.header.stamp = get_node()->now();
     std::visit(
       Visitor{
         [this](auto & sensor)
         {
           sensor_msgs::msg::NavSatFix message;
-          sensor.get_values_as_message(realtime_publisher_->msg_);
+          sensor.get_values_as_message(state_message_);
         },
         [](std::monostate &) {}},
       gps_sensor_);
-    realtime_publisher_->unlockAndPublish();
+    realtime_publisher_->try_publish(state_message_);
   }
   return controller_interface::return_type::OK;
 }

imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp

@@ -61,6 +61,7 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface
   using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::Imu>;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr sensor_state_publisher_;
   std::unique_ptr<StatePublisher> realtime_publisher_;
+  sensor_msgs::msg::Imu state_message_;
 };
 
 }  // namespace imu_sensor_broadcaster

imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

@@ -62,18 +62,15 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
     return CallbackReturn::ERROR;
   }
 
-  realtime_publisher_->lock();
-  realtime_publisher_->msg_.header.frame_id = params_.frame_id;
+  state_message_.header.frame_id = params_.frame_id;
   // convert double vector to fixed-size array in the message
   for (size_t i = 0; i < 9; ++i)
   {
-    realtime_publisher_->msg_.orientation_covariance[i] = params_.static_covariance_orientation[i];
-    realtime_publisher_->msg_.angular_velocity_covariance[i] =
-      params_.static_covariance_angular_velocity[i];
-    realtime_publisher_->msg_.linear_acceleration_covariance[i] =
+    state_message_.orientation_covariance[i] = params_.static_covariance_orientation[i];
+    state_message_.angular_velocity_covariance[i] = params_.static_covariance_angular_velocity[i];
+    state_message_.linear_acceleration_covariance[i] =
       params_.static_covariance_linear_acceleration[i];
   }
-  realtime_publisher_->unlock();
 
   RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");
   return CallbackReturn::SUCCESS;
@@ -113,11 +110,11 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate(
 controller_interface::return_type IMUSensorBroadcaster::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  if (realtime_publisher_ && realtime_publisher_->trylock())
+  if (realtime_publisher_)
   {
-    realtime_publisher_->msg_.header.stamp = time;
-    imu_sensor_->get_values_as_message(realtime_publisher_->msg_);
-    realtime_publisher_->unlockAndPublish();
+    state_message_.header.stamp = time;
+    imu_sensor_->get_values_as_message(state_message_);
+    realtime_publisher_->try_publish(state_message_);
   }
 
   return controller_interface::return_type::OK;

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -105,6 +105,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> joint_state_publisher_;
   std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>
     realtime_joint_state_publisher_;
+  sensor_msgs::msg::JointState joint_state_msg_;
 
   //  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.
@@ -113,6 +114,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
     dynamic_joint_state_publisher_;
   std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
     realtime_dynamic_joint_state_publisher_;
+  control_msgs::msg::DynamicJointState dynamic_joint_state_msg_;
 
   urdf::Model model_;
   bool is_model_loaded_ = false;