Test lock_free queue

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_buffer.hpp"
+#include "realtime_tools/lock_free_queue.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
@@ -122,8 +122,8 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
-  realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
-
+  realtime_tools::LockFreeSPSCQueue<std::shared_ptr<TwistStamped>, 10> received_velocity_msg_ptr_;
+  std::shared_ptr<TwistStamped> last_command_msg_;
   std::queue<std::array<double, 2>> previous_two_commands_;
   // speed limiters
   std::unique_ptr<SpeedLimiter> limiter_linear_;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -111,27 +111,27 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
     return controller_interface::return_type::OK;
   }
 
-  const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
-
-  if (command_msg_ptr == nullptr)
+  // 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.");
     return controller_interface::return_type::ERROR;
   }
 
-  const auto age_of_last_command = time - command_msg_ptr->header.stamp;
+  const auto age_of_last_command = time - last_command_msg_->header.stamp;
   // Brake if cmd_vel has timeout, override the stored command
   if (age_of_last_command > cmd_vel_timeout_)
   {
     reference_interfaces_[0] = 0.0;
     reference_interfaces_[1] = 0.0;
   }
   else if (
-    std::isfinite(command_msg_ptr->twist.linear.x) &&
-    std::isfinite(command_msg_ptr->twist.angular.z))
+    std::isfinite(last_command_msg_->twist.linear.x) &&
+    std::isfinite(last_command_msg_->twist.angular.z))
   {
-    reference_interfaces_[0] = command_msg_ptr->twist.linear.x;
-    reference_interfaces_[1] = command_msg_ptr->twist.angular.z;
+    reference_interfaces_[0] = last_command_msg_->twist.linear.x;
+    reference_interfaces_[1] = last_command_msg_->twist.angular.z;
   }
   else
   {
@@ -425,6 +425,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
         limited_velocity_publisher_);
   }
 
+  last_command_msg_ = std::make_shared<TwistStamped>();
+  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;
+  }
+
   // initialize command subscriber
   velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
     DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
@@ -450,7 +457,17 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
         cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) ||
         current_time_diff < cmd_vel_timeout_)
       {
-        received_velocity_msg_ptr_.writeFromNonRT(msg);
+        for (size_t i = 0; i < 5; ++i)
+        {
+          if (received_velocity_msg_ptr_.bounded_push(msg))
+          {
+            break;
+          }
+          RCLCPP_WARN(
+            get_node()->get_logger(),
+            "Velocity command could not be stored in the queue, trying again");
+          std::this_thread::sleep_for(100us);
+        }
       }
       else
       {
@@ -625,18 +642,8 @@ void DiffDriveController::reset_buffers()
   previous_two_commands_.push({{0.0, 0.0}});
   previous_two_commands_.push({{0.0, 0.0}});
 
-  // Fill RealtimeBuffer with NaNs so it will contain a known value
-  // but still indicate that no command has yet been sent.
-  received_velocity_msg_ptr_.reset();
-  std::shared_ptr<TwistStamped> empty_msg_ptr = std::make_shared<TwistStamped>();
-  empty_msg_ptr->header.stamp = get_node()->now();
-  empty_msg_ptr->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
-  empty_msg_ptr->twist.linear.y = std::numeric_limits<double>::quiet_NaN();
-  empty_msg_ptr->twist.linear.z = std::numeric_limits<double>::quiet_NaN();
-  empty_msg_ptr->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
-  empty_msg_ptr->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
-  empty_msg_ptr->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
-  received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr);
+  std::shared_ptr<TwistStamped> dummy;
+  (void)received_velocity_msg_ptr_.get_latest(dummy);
 }
 
 void DiffDriveController::halt()

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -47,7 +47,9 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
   using DiffDriveController::DiffDriveController;
   std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
   {
-    return *(received_velocity_msg_ptr_.readFromNonRT());
+    std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
+    (void)received_velocity_msg_ptr_.get_latest(ret);
+    return ret;
   }
 
   /**