Update realtime containers (backport #1721)

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -103,13 +103,13 @@ TEST_F(AckermannSteeringControllerTest, activate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   // check that the message is reset
-  auto msg = controller_->input_ref_.readFromNonRT();
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.z));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.z));
+  auto msg = controller_->input_ref_.get();
+  EXPECT_TRUE(std::isnan(msg.twist.linear.x));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.y));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.z));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.x));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.y));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.z));
 }
 
 TEST_F(AckermannSteeringControllerTest, update_success)
@@ -161,11 +161,11 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
   ASSERT_FALSE(controller_->is_in_chained_mode());
 
   // set command statically
-  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  msg->header.stamp = controller_->get_node()->now();
-  msg->twist.linear.x = 0.1;
-  msg->twist.angular.z = 0.2;
-  controller_->input_ref_.writeFromNonRT(msg);
+  ControllerReferenceMsg msg;
+  msg.header.stamp = controller_->get_node()->now();
+  msg.twist.linear.x = 0.1;
+  msg.twist.angular.z = 0.2;
+  controller_->input_ref_.set(msg);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -185,7 +185,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
     controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
     1.4179821977774734, COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -225,7 +225,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
     controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(),
     1.4179821977774734, COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -249,8 +249,9 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
   ControllerStateMsg msg;
   subscribe_and_get_messages(msg);
 
-  EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1);
-  EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3);
+  // never received a valid command, linear velocity should have been reset
+  EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.0);
+  EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.0);
   EXPECT_EQ(msg.steering_angle_command[0], 2.2);
   EXPECT_EQ(msg.steering_angle_command[1], 4.4);
 

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -31,8 +31,8 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 
 namespace admittance_controller
@@ -125,15 +125,16 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // admittance parameters
   std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
 
-  // ROS messages
-  std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
-
-  // real-time buffer
-  realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
+  // real-time boxes
+  realtime_tools::RealtimeThreadSafeBox<trajectory_msgs::msg::JointTrajectoryPoint>
     input_joint_command_;
-  realtime_tools::RealtimeBuffer<geometry_msgs::msg::WrenchStamped> input_wrench_command_;
+  realtime_tools::RealtimeThreadSafeBox<geometry_msgs::msg::WrenchStamped> input_wrench_command_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
 
+  // save the last commands in case of unable to get value from box
+  trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_;
+  geometry_msgs::msg::WrenchStamped wrench_command_msg_;
+
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_;
   trajectory_msgs::msg::JointTrajectoryPoint last_reference_;
 

admittance_controller/src/admittance_controller.cpp

@@ -25,6 +25,27 @@
 #include "geometry_msgs/msg/wrench.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
+namespace
+{  // utility
+
+// called from RT control loop
+void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & msg)
+{
+  msg.positions.clear();
+  msg.velocities.clear();
+}
+
+// called from RT control loop
+void reset_wrench_msg(
+  geometry_msgs::msg::WrenchStamped & msg,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
+{
+  msg.header.stamp = node->now();
+  msg.wrench = geometry_msgs::msg::Wrench();
+}
+
+}  // namespace
+
 namespace admittance_controller
 {
 
@@ -133,7 +154,6 @@ AdmittanceController::on_export_reference_interfaces()
   reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits<double>::quiet_NaN());
   position_reference_ = {};
   velocity_reference_ = {};
-  input_wrench_command_.reset();
 
   // assign reference interfaces
   auto index = 0ul;
@@ -286,7 +306,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
   // setup subscribers and publishers
   auto joint_command_callback =
     [this](const std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> msg)
-  { input_joint_command_.writeFromNonRT(msg); };
+  { input_joint_command_.set(*msg); };
   input_joint_command_subscriber_ =
     get_node()->create_subscription<trajectory_msgs::msg::JointTrajectoryPoint>(
       "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback);
@@ -307,8 +327,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
             msg.header.frame_id.c_str(), admittance_->parameters_.ft_sensor.frame.id.c_str());
           return;
         }
-        input_wrench_command_.writeFromNonRT(msg);
+        input_wrench_command_.set(msg);
       });
+
   s_publisher_ = get_node()->create_publisher<control_msgs::msg::AdmittanceControllerState>(
     "~/status", rclcpp::SystemDefaultsQoS());
   state_publisher_ =
@@ -390,6 +411,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
       return controller_interface::CallbackReturn::ERROR;
     }
   }
+  reset_controller_reference_msg(joint_command_msg_);
+  reset_wrench_msg(wrench_command_msg_, get_node());
 
   // Use current joint_state as a default reference
   last_reference_ = joint_state_;
@@ -408,26 +431,29 @@ controller_interface::return_type AdmittanceController::update_reference_from_su
   {
     return controller_interface::return_type::ERROR;
   }
-
-  joint_command_msg_ = *input_joint_command_.readFromRT();
+  auto joint_command_msg_op = input_joint_command_.try_get();
+  if (joint_command_msg_op.has_value())
+  {
+    joint_command_msg_ = joint_command_msg_op.value();
+  }
 
   // if message exists, load values into references
-  if (joint_command_msg_.get())
+  if (!joint_command_msg_.positions.empty() || !joint_command_msg_.velocities.empty())
   {
     for (const auto & interface : admittance_->parameters_.chainable_command_interfaces)
     {
       if (interface == hardware_interface::HW_IF_POSITION)
       {
-        for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i)
+        for (size_t i = 0; i < joint_command_msg_.positions.size(); ++i)
         {
-          position_reference_[i].get() = joint_command_msg_->positions[i];
+          position_reference_[i].get() = joint_command_msg_.positions[i];
         }
       }
       else if (interface == hardware_interface::HW_IF_VELOCITY)
       {
-        for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i)
+        for (size_t i = 0; i < joint_command_msg_.velocities.size(); ++i)
         {
-          velocity_reference_[i].get() = joint_command_msg_->velocities[i];
+          velocity_reference_[i].get() = joint_command_msg_.velocities[i];
         }
       }
     }
@@ -451,7 +477,13 @@ controller_interface::return_type AdmittanceController::update_and_write_command
   // get all controller inputs
   read_state_from_hardware(joint_state_, ft_values_);
 
-  auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench);
+  auto wrench_command_op = input_wrench_command_.try_get();
+  if (wrench_command_op.has_value())
+  {
+    wrench_command_msg_ = wrench_command_op.value();
+  }
+
+  auto offsetted_ft_values = add_wrenches(ft_values_, wrench_command_msg_.wrench);
 
   // apply admittance control to reference to determine desired state
   admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_);
@@ -498,6 +530,9 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate(
   release_interfaces();
   admittance_->reset(num_joints_);
 
+  reset_controller_reference_msg(joint_command_msg_);
+  reset_wrench_msg(wrench_command_msg_, get_node());
+
   return CallbackReturn::SUCCESS;
 }
 

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -89,13 +89,13 @@ TEST_F(BicycleSteeringControllerTest, activate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   // check that the message is reset
-  auto msg = controller_->input_ref_.readFromNonRT();
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.linear.z));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.z));
+  auto msg = controller_->input_ref_.get();
+  EXPECT_TRUE(std::isnan(msg.twist.linear.x));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.y));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.z));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.x));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.y));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.z));
 }
 
 TEST_F(BicycleSteeringControllerTest, update_success)
@@ -147,11 +147,11 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
   ASSERT_FALSE(controller_->is_in_chained_mode());
 
   // set command statically
-  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  msg->header.stamp = controller_->get_node()->now();
-  msg->twist.linear.x = 0.1;
-  msg->twist.angular.z = 0.2;
-  controller_->input_ref_.writeFromNonRT(msg);
+  ControllerReferenceMsg msg;
+  msg.header.stamp = controller_->get_node()->now();
+  msg.twist.linear.x = 0.1;
+  msg.twist.angular.z = 0.2;
+  controller_->input_ref_.set(msg);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -164,7 +164,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -197,7 +197,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -236,7 +236,8 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
   ControllerStateMsg msg;
   subscribe_and_get_messages(msg);
 
-  EXPECT_EQ(msg.linear_velocity_command[0], 1.1);
+  // never received a valid command, linear velocity should have been reset
+  EXPECT_EQ(msg.linear_velocity_command[0], 0.0);
   EXPECT_EQ(msg.steering_angle_command[0], 2.2);
 
   publish_commands(0.1, 0.2);

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -32,8 +32,8 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
 // auto-generated by generate_parameter_library
@@ -122,7 +122,10 @@ 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};
+  // the realtime container to exchange the reference from subscriber
+  realtime_tools::RealtimeThreadSafeBox<TwistStamped> received_velocity_msg_;
+  // save the last reference in case of unable to get value from box
+  TwistStamped command_msg_;
 
   std::queue<std::array<double, 2>> previous_two_commands_;
   // speed limiters

diff_drive_controller/src/diff_drive_controller.cpp

@@ -107,27 +107,24 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
 {
   auto logger = get_node()->get_logger();
 
-  const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
-
-  if (command_msg_ptr == nullptr)
+  auto current_ref_op = received_velocity_msg_.try_get();
+  if (current_ref_op.has_value())
   {
-    RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
-    return controller_interface::return_type::ERROR;
+    command_msg_ = current_ref_op.value();
   }
 
-  const auto age_of_last_command = time - command_msg_ptr->header.stamp;
+  const auto age_of_last_command = time - 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(command_msg_.twist.linear.x) && std::isfinite(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] = command_msg_.twist.linear.x;
+    reference_interfaces_[1] = command_msg_.twist.angular.z;
   }
   else
   {
@@ -458,7 +455,7 @@ 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);
+        received_velocity_msg_.set(*msg);
       }
       else
       {
@@ -629,18 +626,16 @@ 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
+  // Fill RealtimeBox 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);
+  command_msg_.header.stamp = get_node()->now();
+  command_msg_.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+  command_msg_.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
+  command_msg_.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
+  command_msg_.twist.angular.x = std::numeric_limits<double>::quiet_NaN();
+  command_msg_.twist.angular.y = std::numeric_limits<double>::quiet_NaN();
+  command_msg_.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+  received_velocity_msg_.set(command_msg_);
 }
 
 void DiffDriveController::halt()

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -45,10 +45,6 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
 {
 public:
   using DiffDriveController::DiffDriveController;
-  std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
-  {
-    return *(received_velocity_msg_ptr_.readFromNonRT());
-  }
 
   /**
    * @brief wait_for_twist block until a new twist is received.