Use a realtime_tools::RealtimePublisher for publishing the state (backport #1822) (#1825)

Author: mergify[bot]

ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp

@@ -42,12 +42,11 @@
 #include <string>
 #include <vector>
 
-#include "controller_interface/controller_interface.hpp"
-#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp/time.hpp"
-#include "rclcpp/duration.hpp"
-#include "std_msgs/msg/float64.hpp"
+#include <realtime_tools/realtime_publisher.hpp>
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/duration.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include "ur_controllers/speed_scaling_state_broadcaster_parameters.hpp"
 
 namespace ur_controllers
@@ -69,13 +68,15 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
 
   controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
+  controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+
   controller_interface::CallbackReturn on_init() override;
 
 protected:
   std::vector<std::string> sensor_names_;
   double publish_rate_;
 
-  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;
   std_msgs::msg::Float64 speed_scaling_state_msg_;
 
   // Parameters from ROS for SpeedScalingStateBroadcaster

ur_controllers/src/speed_scaling_state_broadcaster.cpp

@@ -108,8 +108,8 @@ SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*prev
   RCLCPP_INFO(get_node()->get_logger(), "Publisher rate set to : %.1f Hz", publish_rate_);
 
   try {
-    speed_scaling_state_publisher_ =
-        get_node()->create_publisher<std_msgs::msg::Float64>("~/speed_scaling", rclcpp::SystemDefaultsQoS());
+    speed_scaling_state_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64>>(
+        get_node()->create_publisher<std_msgs::msg::Float64>("~/speed_scaling", rclcpp::SystemDefaultsQoS()));
   } catch (const std::exception& e) {
     // get_node() may throw, logging raw here
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
@@ -138,11 +138,18 @@ controller_interface::return_type SpeedScalingStateBroadcaster::update(const rcl
     speed_scaling_state_msg_.data = state_interfaces_[0].get_optional().value_or(1.0) * 100.0;
 
     // publish
-    speed_scaling_state_publisher_->publish(speed_scaling_state_msg_);
+    speed_scaling_state_publisher_->try_publish(speed_scaling_state_msg_);
   }
   return controller_interface::return_type::OK;
 }
 
+controller_interface::CallbackReturn
+SpeedScalingStateBroadcaster::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  speed_scaling_state_publisher_.reset();
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"