Use new RT publisher API in PID class (#394) (#407)

Co-authored-by: Christoph Fröhlich <[email protected]>

Author: mergify[bot]

control_toolbox/include/control_toolbox/pid_ros.hpp

@@ -544,6 +544,7 @@ class PidROS
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_;
 
   std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::PidState>> rt_state_pub_;
+  control_msgs::msg::PidState pid_state_msg_;
   std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> state_pub_;
 
   Pid pid_;

control_toolbox/src/pid_ros.cpp

@@ -497,23 +497,20 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
   // Publish controller state if configured
   if (rt_state_pub_)
   {
-    if (rt_state_pub_->trylock())
-    {
-      rt_state_pub_->msg_.header.stamp = rclcpp::Clock().now();
-      rt_state_pub_->msg_.timestep = dt;
-      rt_state_pub_->msg_.error = error;
-      rt_state_pub_->msg_.error_dot = d_error;
-      rt_state_pub_->msg_.p_error = p_error;
-      rt_state_pub_->msg_.i_error = i_term;
-      rt_state_pub_->msg_.d_error = d_error;
-      rt_state_pub_->msg_.p_term = gains.p_gain_;
-      rt_state_pub_->msg_.i_term = gains.i_gain_;
-      rt_state_pub_->msg_.d_term = gains.d_gain_;
-      rt_state_pub_->msg_.i_max = gains.i_max_;
-      rt_state_pub_->msg_.i_min = gains.i_min_;
-      rt_state_pub_->msg_.output = cmd;
-      rt_state_pub_->unlockAndPublish();
-    }
+    pid_state_msg_.header.stamp = rclcpp::Clock().now();
+    pid_state_msg_.timestep = dt;
+    pid_state_msg_.error = error;
+    pid_state_msg_.error_dot = d_error;
+    pid_state_msg_.p_error = p_error;
+    pid_state_msg_.i_error = i_term;
+    pid_state_msg_.d_error = d_error;
+    pid_state_msg_.p_term = gains.p_gain_;
+    pid_state_msg_.i_term = gains.i_gain_;
+    pid_state_msg_.d_term = gains.d_gain_;
+    pid_state_msg_.i_max = gains.i_max_;
+    pid_state_msg_.i_min = gains.i_min_;
+    pid_state_msg_.output = cmd;
+    rt_state_pub_->try_publish(pid_state_msg_);
   }
 }