Replaced moprim_cmd_mutex_ with realtime_tools::LockFreeSPSCQueue<std::array<double, 25>, 1> → therefore changed command and state interfaces from std::vector<double> to std::array<double, 25>.

Author: mathias31415

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -48,6 +48,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+#include <array>
 
 // ros2_control hardware_interface
 #include "hardware_interface/hardware_info.hpp"
@@ -67,10 +68,11 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "control_msgs/msg/motion_primitive.hpp"
+#include <realtime_tools/lock_free_queue.hpp>
 
 // Motion primitives controller
 #include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp"
-#include "control_msgs/msg/motion_primitive.hpp"
 
 using MoprimMotionType = control_msgs::msg::MotionPrimitive;
 using MoprimMotionHelperType = motion_primitives_forward_controller::MotionHelperType;
@@ -292,10 +294,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   // Async thread handling
   std::shared_ptr<std::thread> async_moprim_cmd_thread_;
   std::atomic_bool async_moprim_thread_shutdown_;
-  std::mutex moprim_cmd_mutex_;
-
-  // Command buffer for thread-safe communication
-  std::vector<double> pending_moprim_cmd_;
+  realtime_tools::LockFreeSPSCQueue<std::array<double, 25>, 1> moprim_cmd_queue_;
   std::atomic_bool new_moprim_cmd_available_;
 
   // Status for communication with controller
@@ -305,8 +304,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   std::atomic_bool ready_for_new_moprim_;
 
   // Command and state interfaces for the motion primitives
-  std::vector<double> hw_moprim_commands_;
-  std::vector<double> hw_moprim_states_;
+  // 25 Commands: motion_type + 6 joints + 2*7 positions (goal and via) + blend_radius + velocity + acceleration +
+  // move_time
+  std::array<double, 25> hw_moprim_commands_;
+  // 2 States: execution_status, ready_for_new_primitive
+  std::array<double, 2> hw_moprim_states_;
 
   // flag to put all following primitives into a motion sequence instead of sending single primitives
   std::atomic_bool build_moprim_sequence_{ false };
@@ -315,10 +317,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   void handleMoprimCommands();
   void resetMoprimCmdInterfaces();
   void asyncMoprimCmdThread();
-  void processMoprimMotionCmd(const std::vector<double>& command);
-  bool getMoprimTimeOrVelAndAcc(const std::vector<double>& command, double& velocity, double& acceleration,
+  void processMoprimMotionCmd(const std::array<double, 25>& command);
+  bool getMoprimTimeOrVelAndAcc(const std::array<double, 25>& command, double& velocity, double& acceleration,
                                 double& move_time);
-  bool getMoprimVelAndAcc(const std::vector<double>& command, double& velocity, double& acceleration,
+  bool getMoprimVelAndAcc(const std::array<double, 25>& command, double& velocity, double& acceleration,
                           double& move_time);
   void quaternionToRotVec(double qx, double qy, double qz, double qw, double& rx, double& ry, double& rz);
 

ur_robot_driver/src/hardware_interface.cpp

@@ -151,11 +151,8 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
   current_moprim_execution_status_ = MoprimExecutionState::IDLE;
   ready_for_new_moprim_ = false;
   motion_primitives_forward_controller_running_ = false;
-  // 2 States: execution_status, ready_for_new_primitive
-  hw_moprim_states_.resize(2, std::numeric_limits<double>::quiet_NaN());
-  // 25 Commands: // motion_type + 6 joints + 2*7 positions (goal and via) + blend_radius + velocity + acceleration +
-  // move_time
-  hw_moprim_commands_.resize(25, std::numeric_limits<double>::quiet_NaN());
+  hw_moprim_states_.fill(std::numeric_limits<double>::quiet_NaN());
+  hw_moprim_commands_.fill(std::numeric_limits<double>::quiet_NaN());
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints) {
     if (joint.command_interfaces.size() != 2) {
@@ -1617,9 +1614,9 @@ void URPositionHardwareInterface::handleMoprimCommands()
         RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Received Motion Primitives STOP command");
         resetMoprimCmdInterfaces();
         build_moprim_sequence_ = false;
-        moprim_sequence_.clear(); // delete motion sequence
+        moprim_sequence_.clear();  // delete motion sequence
         ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL, -1,
-                                           urcl::RobotReceiveTimeout::millisec(2000));
+                                                  urcl::RobotReceiveTimeout::millisec(2000));
         current_moprim_execution_status_ = MoprimExecutionState::STOPPING;
         ready_for_new_moprim_ = false;
         break;
@@ -1634,10 +1631,12 @@ void URPositionHardwareInterface::handleMoprimCommands()
       }
       default:
       {
-        std::lock_guard<std::mutex> guard(moprim_cmd_mutex_);
         if (!new_moprim_cmd_available_) {
-          // Copy command to thread-safe buffer
-          pending_moprim_cmd_ = hw_moprim_commands_;
+          // Push command to thread-safe queue
+          if (!moprim_cmd_queue_.push(hw_moprim_commands_)) {
+            RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Failed to push command to "
+                                                                            "moprim_cmd_queue_");
+          }
           new_moprim_cmd_available_ = true;
           resetMoprimCmdInterfaces();
         }
@@ -1661,13 +1660,12 @@ void URPositionHardwareInterface::asyncMoprimCmdThread()
   while (!async_moprim_thread_shutdown_) {
     // Check for new commands
     if (new_moprim_cmd_available_) {
-      // RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "[asyncMoprimCmdThread] New command available");
-      std::vector<double> current_command;
-      {
-        std::lock_guard<std::mutex> guard(moprim_cmd_mutex_);
-        current_command = pending_moprim_cmd_;
-        new_moprim_cmd_available_ = false;
+      std::array<double, 25> current_command;
+      if (!moprim_cmd_queue_.pop(current_command)) {
+        RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Failed to pop the command from "
+                                                                        "moprim_cmd_queue_.");
       }
+      new_moprim_cmd_available_ = false;
 
       // Process the command
       processMoprimMotionCmd(current_command);
@@ -1679,9 +1677,9 @@ void URPositionHardwareInterface::asyncMoprimCmdThread()
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "[asyncMoprimCmdThread] Exiting");
 }
 
-void URPositionHardwareInterface::processMoprimMotionCmd(const std::vector<double>& command)
+void URPositionHardwareInterface::processMoprimMotionCmd(const std::array<double, 25>& command)
 {
-  if (command.empty() || std::isnan(command[0])) {
+  if (std::isnan(command[0])) {
     return;
   }
   double velocity, acceleration, move_time;
@@ -1906,7 +1904,7 @@ void URPositionHardwareInterface::quaternionToRotVec(double qx, double qy, doubl
   rz = axis.z() * angle;  // rz
 }
 
-bool URPositionHardwareInterface::getMoprimTimeOrVelAndAcc(const std::vector<double>& command, double& velocity,
+bool URPositionHardwareInterface::getMoprimTimeOrVelAndAcc(const std::array<double, 25>& command, double& velocity,
                                                            double& acceleration, double& move_time)
 {
   // Check if move_time is valid
@@ -1931,7 +1929,7 @@ bool URPositionHardwareInterface::getMoprimTimeOrVelAndAcc(const std::vector<dou
   }
 }
 
-bool URPositionHardwareInterface::getMoprimVelAndAcc(const std::vector<double>& command, double& velocity,
+bool URPositionHardwareInterface::getMoprimVelAndAcc(const std::array<double, 25>& command, double& velocity,
                                                      double& acceleration, double& move_time)
 {
   // Check if velocity and acceleration are valid