fix: new api for realtime publishers in kilted

Author: danielsanjosepro

include/crisp_controllers/utils/ros2_version.hpp

@@ -38,3 +38,5 @@
 #else
 #define HAS_ROS2_CONTROL_INTROSPECTION 0
 #endif
+
+#define REALTIME_TOOLS_NEW_API ROS2_VERSION_ABOVE_JAZZY

src/pose_broadcaster.cpp

@@ -70,19 +70,24 @@ PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & peri
   bool should_publish =
     (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0);
   if (should_publish && realtime_pose_publisher_) {
+    geometry_msgs::msg::PoseStamped pose_msg;
+    pose_msg.header.stamp = time;
+    pose_msg.header.frame_id = params_.base_frame;
+    pose_msg.pose.position.x = current_pose.translation()[0];
+    pose_msg.pose.position.y = current_pose.translation()[1];
+    pose_msg.pose.position.z = current_pose.translation()[2];
+    pose_msg.pose.orientation.x = current_quaternion.x();
+    pose_msg.pose.orientation.y = current_quaternion.y();
+    pose_msg.pose.orientation.z = current_quaternion.z();
+    pose_msg.pose.orientation.w = current_quaternion.w();
+
+#if REALTIME_TOOLS_NEW_API
+    if (realtime_pose_publisher_->try_publish(pose_msg)) {
+#else
     if (realtime_pose_publisher_->trylock()) {
-      auto & pose_msg = realtime_pose_publisher_->msg_;
-      pose_msg.header.stamp = time;
-      pose_msg.header.frame_id = params_.base_frame;
-      pose_msg.pose.position.x = current_pose.translation()[0];
-      pose_msg.pose.position.y = current_pose.translation()[1];
-      pose_msg.pose.position.z = current_pose.translation()[2];
-      pose_msg.pose.orientation.x = current_quaternion.x();
-      pose_msg.pose.orientation.y = current_quaternion.y();
-      pose_msg.pose.orientation.z = current_quaternion.z();
-      pose_msg.pose.orientation.w = current_quaternion.w();
+      realtime_pose_publisher_->msg_ = pose_msg;
       realtime_pose_publisher_->unlockAndPublish();
-
+#endif
       publish_elapsed_ = publish_elapsed_ - publish_interval_;
       // clamp to publish only 1 time even if missed multiple intervals
       publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);

src/twist_broadcaster.cpp

@@ -77,18 +77,23 @@ TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & per
     (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0);
 
   if (should_publish && realtime_twist_publisher_) {
+    geometry_msgs::msg::TwistStamped twist_msg;
+    twist_msg.header.stamp = time;
+    twist_msg.header.frame_id = params_.end_effector_frame;
+    twist_msg.twist.linear.x = current_velocity.linear()[0];
+    twist_msg.twist.linear.y = current_velocity.linear()[1];
+    twist_msg.twist.linear.z = current_velocity.linear()[2];
+    twist_msg.twist.angular.x = current_velocity.angular()[0];
+    twist_msg.twist.angular.y = current_velocity.angular()[1];
+    twist_msg.twist.angular.z = current_velocity.angular()[2];
+
+#if REALTIME_TOOLS_NEW_API
+    if (realtime_twist_publisher_->try_publish(twist_msg)) {
+#else
     if (realtime_twist_publisher_->trylock()) {
-      auto & twist_msg = realtime_twist_publisher_->msg_;
-      twist_msg.header.stamp = time;
-      twist_msg.header.frame_id = params_.end_effector_frame;
-      twist_msg.twist.linear.x = current_velocity.linear()[0];
-      twist_msg.twist.linear.y = current_velocity.linear()[1];
-      twist_msg.twist.linear.z = current_velocity.linear()[2];
-      twist_msg.twist.angular.x = current_velocity.angular()[0];
-      twist_msg.twist.angular.y = current_velocity.angular()[1];
-      twist_msg.twist.angular.z = current_velocity.angular()[2];
+      realtime_twist_publisher_->msg_ = twist_msg;
       realtime_twist_publisher_->unlockAndPublish();
-
+#endif
       publish_elapsed_ = publish_elapsed_ - publish_interval_;
       // clamp to publish only 1 time even if missed multiple intervals
       publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);