Diff_drive: Use lock_free queue instead of RealtimeBox

Author: christophfroehlich

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -32,7 +32,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_box.hpp"
+#include "realtime_tools/lock_free_queue.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
@@ -41,6 +41,7 @@
 
 namespace diff_drive_controller
 {
+using realtime_tools::LockFreeSPSCQueue;
 class DiffDriveController : public controller_interface::ControllerInterface
 {
   using TwistStamped = geometry_msgs::msg::TwistStamped;
@@ -117,7 +118,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
-  realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
+  LockFreeSPSCQueue<std::shared_ptr<TwistStamped>, 10> received_velocity_msg_ptr_;
   std::shared_ptr<TwistStamped> last_command_msg_;
 
   std::queue<TwistStamped> previous_commands_;  // last two commands

diff_drive_controller/src/diff_drive_controller.cpp

@@ -118,10 +118,8 @@ controller_interface::return_type DiffDriveController::update(
     return controller_interface::return_type::OK;
   }
 
-  // if the mutex is unable to lock, last_command_msg_ won't be updated
-  received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
-                                     { last_command_msg_ = msg; });
-
+  // last_command_msg_ won't be updated if the queue is empty
+  (void)received_velocity_msg_ptr_.get_latest(last_command_msg_);
   if (last_command_msg_ == nullptr)
   {
     RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
@@ -395,8 +393,11 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   }
 
   last_command_msg_ = std::make_shared<TwistStamped>();
-  received_velocity_msg_ptr_.set([this](std::shared_ptr<TwistStamped> & stored_value)
-                                 { stored_value = last_command_msg_; });
+  if (!received_velocity_msg_ptr_.bounded_push(last_command_msg_))
+  {
+    RCLCPP_ERROR(logger, "Failed to push anything to the command queue");
+    return controller_interface::CallbackReturn::ERROR;
+  }
   // Fill last two commands with default constructed commands
   previous_commands_.emplace(*last_command_msg_);
   previous_commands_.emplace(*last_command_msg_);
@@ -419,8 +420,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
           "time, this message will only be shown once");
         msg->header.stamp = get_node()->get_clock()->now();
       }
-      received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
-                                     { stored_value = std::move(msg); });
+      (void)received_velocity_msg_ptr_.bounded_push(std::move(msg));
     });
 
   // initialize odometry publisher and message
@@ -573,7 +573,8 @@ bool DiffDriveController::reset()
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
 
-  received_velocity_msg_ptr_.set(nullptr);
+  std::shared_ptr<TwistStamped> dummy;
+  (void)received_velocity_msg_ptr_.get_latest(dummy);
   is_halted = false;
   return true;
 }

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -48,8 +48,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
   std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
   {
     std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
-    received_velocity_msg_ptr_.get(
-      [&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
+    (void)received_velocity_msg_ptr_.get_latest(ret);
     return ret;
   }