From af2a111361be4d525b248f1cce0efb989bb15535 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 30 May 2025 21:05:27 +0000 Subject: [PATCH 01/22] Apply changes to mecanum_drive_controller --- .../mecanum_drive_controller.hpp | 11 +- .../mecanum_drive_controller/odometry.hpp | 7 +- .../src/mecanum_drive_controller.cpp | 30 +++- .../test/test_mecanum_drive_controller.cpp | 134 +++++++++++------- 4 files changed, 115 insertions(+), 67 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 189805c17c..5586ceecf0 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.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 "std_srvs/srv/set_bool.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -109,14 +109,13 @@ class MecanumDriveController : public controller_interface::ChainableControllerI */ std::vector state_joint_names_; - // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. - // used for preceding controller - std::vector reference_names_; + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox> input_ref_; + // the reference timeout value from parameters + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // Command subscribers and Controller State, odom state, tf state publishers rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; - rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); using OdomStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr odom_s_publisher_; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 79a1b54846..823764ccfe 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -15,9 +15,10 @@ #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ -#include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_publisher.hpp" +#include +#include + +#include "rclcpp/time.hpp" #define PLANAR_POINT_DIM 3 diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 0d0f1e49f8..a0a7416f97 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -131,7 +131,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( std::shared_ptr msg = std::make_shared(); reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + input_ref_.set([msg](std::shared_ptr & stored_value) + { stored_value = msg; }); try { @@ -231,7 +232,8 @@ void MecanumDriveController::reference_callback(const std::shared_ptr & stored_value) + { stored_value = msg; }); } else { @@ -241,7 +243,10 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - reset_controller_reference_msg(msg, get_node()); + std::shared_ptr emtpy_msg = std::make_shared(); + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.set([emtpy_msg](std::shared_ptr & stored_value) + { stored_value = emtpy_msg; }); } } @@ -304,7 +309,10 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + std::shared_ptr emtpy_msg = std::make_shared(); + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.try_set([emtpy_msg](std::shared_ptr & stored_value) + { stored_value = emtpy_msg; }); return controller_interface::CallbackReturn::SUCCESS; } @@ -333,7 +341,13 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); + std::shared_ptr current_ref; + if (!input_ref_.try_get([&](std::shared_ptr value) + { current_ref = value; })) + { + // reference_interfaces_ will remain unchanged + return controller_interface::return_type::OK; + } const auto age_of_last_command = time - (current_ref)->header.stamp; // send message only if there is no timeout @@ -352,6 +366,9 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set([current_ref](std::shared_ptr & stored_value) + { stored_value = current_ref; }); } } } @@ -368,6 +385,9 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set([current_ref](std::shared_ptr & stored_value) + { stored_value = current_ref; }); } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 00c1f5a74b..e5e452b6df 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -126,10 +126,11 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference 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)); - - ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); + std::shared_ptr msg; + controller_->input_ref_.try_get([&](std::shared_ptr value) + { msg = value; }); + EXPECT_TRUE(std::isnan(msg->twist.linear.x)); + ASSERT_TRUE(std::isnan(msg->twist.angular.z)); } TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) @@ -247,7 +248,9 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = *(controller_->input_ref_.readFromNonRT()); + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); auto old_timestamp = reference->header.stamp; EXPECT_TRUE(std::isnan(reference->twist.linear.x)); EXPECT_TRUE(std::isnan(reference->twist.linear.y)); @@ -260,10 +263,10 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); controller_->wait_for_commands(executor); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); + ASSERT_EQ(old_timestamp, reference->header.stamp); + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); } // when time stamp is zero expect that time stamp is set to current time stamp @@ -278,11 +281,13 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + auto old_timestamp = reference->header.stamp; + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -290,13 +295,16 @@ TEST_F( publish_commands(rclcpp::Time(0)); controller_->wait_for_commands(executor); - ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); - EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + + ASSERT_EQ(old_timestamp.sec, reference->header.stamp.sec); + EXPECT_FALSE(std::isnan(reference->twist.linear.x)); + EXPECT_FALSE(std::isnan(reference->twist.angular.z)); + EXPECT_EQ(reference->twist.linear.x, 1.5); + EXPECT_EQ(reference->twist.linear.y, 0.0); + EXPECT_EQ(reference->twist.angular.z, 0.0); + EXPECT_NE(reference->header.stamp.sec, 0.0); } // when the reference_msg has valid timestamp then the timeout check in reference_callback() @@ -311,9 +319,11 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -321,11 +331,13 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + EXPECT_FALSE(std::isnan(reference->twist.linear.x)); + EXPECT_FALSE(std::isnan(reference->twist.angular.z)); + EXPECT_EQ(reference->twist.linear.x, 1.5); + EXPECT_EQ(reference->twist.linear.y, 0.0); + EXPECT_EQ(reference->twist.angular.z, 0.0); } // when not in chainable mode and ref_msg_timedout expect @@ -364,13 +376,17 @@ TEST_F( msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->input_ref_.set([msg](std::shared_ptr & stored_value) + { stored_value = msg; }); + + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + const auto age_of_last_command = controller_->get_node()->now() - reference->header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -394,13 +410,16 @@ TEST_F( msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->input_ref_.set([msg_2](std::shared_ptr & stored_value) + { stored_value = msg_2; }); + + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + const auto age_of_last_command_2 = controller_->get_node()->now() - reference->header.stamp; // age_of_last_command_2 < ref_timeout_ ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -414,7 +433,7 @@ TEST_F( // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * // 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -510,12 +529,16 @@ TEST_F( msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->input_ref_.set([msg](std::shared_ptr & stored_value) + { stored_value = msg; }); + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + + const auto age_of_last_command = controller_->get_node()->now() - reference->header.stamp; ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -529,7 +552,7 @@ TEST_F( // velocity_in_center_frame_angular_z_); // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + ASSERT_TRUE(std::isnan(reference->twist.linear.x)); } TEST_F( @@ -542,9 +565,12 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + std::shared_ptr reference; + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. @@ -553,13 +579,15 @@ TEST_F( publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + controller_->input_ref_.get([&](std::shared_ptr value) + { reference = value; }); + + EXPECT_FALSE(std::isnan(reference->twist.linear.x)); + EXPECT_FALSE(std::isnan(reference->twist.linear.y)); + EXPECT_FALSE(std::isnan(reference->twist.angular.z)); + EXPECT_EQ(reference->twist.linear.x, 1.5); + EXPECT_EQ(reference->twist.linear.y, 0.0); + EXPECT_EQ(reference->twist.angular.z, 0.0); } TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) From ecfa2286a2b9788b99d041b0abaea538dc5359df Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 31 May 2025 16:53:45 +0000 Subject: [PATCH 02/22] Use msg instead of shared_ptr in box --- .../mecanum_drive_controller.hpp | 2 +- .../src/mecanum_drive_controller.cpp | 80 ++++--- .../test/test_mecanum_drive_controller.cpp | 199 ++++++++---------- 3 files changed, 127 insertions(+), 154 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 5586ceecf0..38083b35e2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -110,7 +110,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::vector state_joint_names_; // the RT Box containing the command message - realtime_tools::RealtimeThreadSafeBox> input_ref_; + realtime_tools::RealtimeThreadSafeBox input_ref_; // the reference timeout value from parameters rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index a0a7416f97..93675c480c 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -33,16 +33,15 @@ using ControllerReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerReferenceMsg & msg, const std::shared_ptr & node) { - msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); + msg.header.stamp = node->now(); + msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } } // namespace @@ -129,10 +128,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg; reset_controller_reference_msg(msg, get_node()); - input_ref_.set([msg](std::shared_ptr & stored_value) - { stored_value = msg; }); + input_ref_.set(msg); try { @@ -232,8 +230,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptr & stored_value) - { stored_value = msg; }); + input_ref_.set(*msg); } else { @@ -243,10 +240,9 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - std::shared_ptr emtpy_msg = std::make_shared(); + ControllerReferenceMsg emtpy_msg; reset_controller_reference_msg(emtpy_msg, get_node()); - input_ref_.set([emtpy_msg](std::shared_ptr & stored_value) - { stored_value = emtpy_msg; }); + input_ref_.set(emtpy_msg); } } @@ -308,11 +304,11 @@ bool MecanumDriveController::on_set_chained_mode(bool /*chained_mode*/) { return controller_interface::CallbackReturn MecanumDriveController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - std::shared_ptr emtpy_msg = std::make_shared(); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + ControllerReferenceMsg emtpy_msg; reset_controller_reference_msg(emtpy_msg, get_node()); - input_ref_.try_set([emtpy_msg](std::shared_ptr & stored_value) - { stored_value = emtpy_msg; }); + input_ref_.try_set(emtpy_msg); return controller_interface::CallbackReturn::SUCCESS; } @@ -341,53 +337,51 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - std::shared_ptr current_ref; - if (!input_ref_.try_get([&](std::shared_ptr value) - { current_ref = value; })) + auto current_ref_op = input_ref_.try_get(); + if (!current_ref_op.has_value()) { // reference_interfaces_ will remain unchanged return controller_interface::return_type::OK; } - const auto age_of_last_command = time - (current_ref)->header.stamp; + auto current_ref = current_ref_op.value(); + const auto age_of_last_command = time - current_ref.header.stamp; - // send message only if there is no timeout + // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { if ( - !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) + !std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y) && + !std::isnan(current_ref.twist.angular.z)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.linear.y; - reference_interfaces_[2] = current_ref->twist.angular.z; + reference_interfaces_[0] = current_ref.twist.linear.x; + reference_interfaces_[1] = current_ref.twist.linear.y; + reference_interfaces_[2] = current_ref.twist.angular.z; if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set([current_ref](std::shared_ptr & stored_value) - { stored_value = current_ref; }); + input_ref_.try_set(current_ref); } } } else { if ( - !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) + !std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y) && + !std::isnan(current_ref.twist.angular.z)) { reference_interfaces_[0] = 0.0; reference_interfaces_[1] = 0.0; reference_interfaces_[2] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set([current_ref](std::shared_ptr & stored_value) - { stored_value = current_ref; }); + input_ref_.try_set(current_ref); } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index e5e452b6df..a0a7c0a61f 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -126,11 +126,9 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - std::shared_ptr msg; - controller_->input_ref_.try_get([&](std::shared_ptr value) - { msg = value; }); - EXPECT_TRUE(std::isnan(msg->twist.linear.x)); - ASSERT_TRUE(std::isnan(msg->twist.angular.z)); + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + ASSERT_TRUE(std::isnan(msg.twist.angular.z)); } TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) @@ -248,13 +246,11 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - auto old_timestamp = reference->header.stamp; - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.linear.y)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + auto old_timestamp = reference.header.stamp; + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -263,10 +259,10 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); controller_->wait_for_commands(executor); - ASSERT_EQ(old_timestamp, reference->header.stamp); - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.linear.y)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + ASSERT_EQ(old_timestamp, reference.header.stamp); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); } // when time stamp is zero expect that time stamp is set to current time stamp @@ -281,13 +277,11 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - auto old_timestamp = reference->header.stamp; - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.linear.y)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + auto old_timestamp = reference.header.stamp; + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -295,16 +289,15 @@ TEST_F( publish_commands(rclcpp::Time(0)); controller_->wait_for_commands(executor); - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - - ASSERT_EQ(old_timestamp.sec, reference->header.stamp.sec); - EXPECT_FALSE(std::isnan(reference->twist.linear.x)); - EXPECT_FALSE(std::isnan(reference->twist.angular.z)); - EXPECT_EQ(reference->twist.linear.x, 1.5); - EXPECT_EQ(reference->twist.linear.y, 0.0); - EXPECT_EQ(reference->twist.angular.z, 0.0); - EXPECT_NE(reference->header.stamp.sec, 0.0); + reference = controller_->input_ref_.get(); + + ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); + EXPECT_NE(reference.header.stamp.sec, 0.0); } // when the reference_msg has valid timestamp then the timeout check in reference_callback() @@ -319,11 +312,9 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called // reference_msg is published with provided time stamp when publish_commands( time_stamp) @@ -331,13 +322,12 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - EXPECT_FALSE(std::isnan(reference->twist.linear.x)); - EXPECT_FALSE(std::isnan(reference->twist.angular.z)); - EXPECT_EQ(reference->twist.linear.x, 1.5); - EXPECT_EQ(reference->twist.linear.y, 0.0); - EXPECT_EQ(reference->twist.angular.z, 0.0); + reference = controller_->input_ref_.get(); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } // when not in chainable mode and ref_msg_timedout expect @@ -366,27 +356,24 @@ TEST_F( // set command statically joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg, msg_2; - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set([msg](std::shared_ptr & stored_value) - { stored_value = msg; }); + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - const auto age_of_last_command = controller_->get_node()->now() - reference->header.stamp; + auto reference = controller_->input_ref_.get(); + const auto age_of_last_command = controller_->get_node()->now() - reference.header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -402,24 +389,21 @@ TEST_F( EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set([msg_2](std::shared_ptr & stored_value) - { stored_value = msg_2; }); - - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - const auto age_of_last_command_2 = controller_->get_node()->now() - reference->header.stamp; + msg_2.header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg_2); + + reference = controller_->input_ref_.get(); + const auto age_of_last_command_2 = controller_->get_node()->now() - reference.header.stamp; // age_of_last_command_2 < ref_timeout_ ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -433,7 +417,7 @@ TEST_F( // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * // 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -520,25 +504,22 @@ TEST_F( joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - std::shared_ptr msg = std::make_shared(); - - msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.set([msg](std::shared_ptr & stored_value) - { stored_value = msg; }); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - - const auto age_of_last_command = controller_->get_node()->now() - reference->header.stamp; + ControllerReferenceMsg msg; + + msg.header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + auto reference = controller_->input_ref_.get(); + + const auto age_of_last_command = controller_->get_node()->now() - reference.header.stamp; ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ(reference->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -552,7 +533,8 @@ TEST_F( // velocity_in_center_frame_angular_z_); // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); - ASSERT_TRUE(std::isnan(reference->twist.linear.x)); + reference = controller_->input_ref_.get(); + ASSERT_TRUE(std::isnan(reference.twist.linear.x)); } TEST_F( @@ -565,12 +547,10 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::shared_ptr reference; - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - EXPECT_TRUE(std::isnan(reference->twist.linear.x)); - EXPECT_TRUE(std::isnan(reference->twist.linear.y)); - EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + auto reference = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. @@ -579,15 +559,14 @@ TEST_F( publish_commands(controller_->get_node()->now()); controller_->wait_for_commands(executor); - controller_->input_ref_.get([&](std::shared_ptr value) - { reference = value; }); - - EXPECT_FALSE(std::isnan(reference->twist.linear.x)); - EXPECT_FALSE(std::isnan(reference->twist.linear.y)); - EXPECT_FALSE(std::isnan(reference->twist.angular.z)); - EXPECT_EQ(reference->twist.linear.x, 1.5); - EXPECT_EQ(reference->twist.linear.y, 0.0); - EXPECT_EQ(reference->twist.angular.z, 0.0); + reference = controller_->input_ref_.get(); + + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.linear.y)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) From 22f6def422d6fe52e59351e224677cfaa33bcfe4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Jun 2025 20:18:11 +0000 Subject: [PATCH 03/22] meanum: keep copy of ref --- .../mecanum_drive_controller.hpp | 2 ++ .../src/mecanum_drive_controller.cpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 38083b35e2..221466eaf0 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -111,6 +111,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // the RT Box containing the command message realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerReferenceMsg last_ref_; // the reference timeout value from parameters rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 93675c480c..1e07b63492 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -338,12 +338,16 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref_op = input_ref_.try_get(); - if (!current_ref_op.has_value()) + ControllerReferenceMsg current_ref; + if (current_ref_op.has_value()) { - // reference_interfaces_ will remain unchanged - return controller_interface::return_type::OK; + current_ref = last_ref_ = current_ref_op.value(); } - auto current_ref = current_ref_op.value(); + else + { + current_ref = last_ref_; + } + const auto age_of_last_command = time - current_ref.header.stamp; // accept message only if there is no timeout From fb6b041adc07dfeabf3e514ddc3d4f5334dd602a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 11:58:21 +0000 Subject: [PATCH 04/22] diff_drive: Use ThreadSafeBox --- .../diff_drive_controller.hpp | 7 ++- .../src/diff_drive_controller.cpp | 46 +++++++++---------- .../test/test_diff_drive_controller.cpp | 4 -- 3 files changed, 28 insertions(+), 29 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index c736c0032a..d7d775de83 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/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::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; + // save the last reference in case of unable to get value from box + TwistStamped last_ref_; std::queue> previous_two_commands_; // speed limiters diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index b855c3acf1..da444a7b3e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -107,27 +107,28 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub { auto logger = get_node()->get_logger(); - const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - - if (command_msg_ptr == nullptr) + auto current_ref_op = received_velocity_msg_.try_get(); + TwistStamped command_msg; + if (current_ref_op.has_value()) + { + command_msg = last_ref_ = current_ref_op.value(); + } + else { - RCLCPP_WARN(logger, "Velocity message received was a nullptr."); - return controller_interface::return_type::ERROR; + command_msg = last_ref_; } - 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)) + else if (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 { @@ -396,7 +397,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 { @@ -567,18 +568,17 @@ 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 empty_msg_ptr = std::make_shared(); - empty_msg_ptr->header.stamp = get_node()->now(); - empty_msg_ptr->twist.linear.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.z = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.z = std::numeric_limits::quiet_NaN(); - received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); + TwistStamped empty_msg; + empty_msg.header.stamp = get_node()->now(); + empty_msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + empty_msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + empty_msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + empty_msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + empty_msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + empty_msg.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(empty_msg); } void DiffDriveController::halt() diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4cd64bdd6d..bd37fcc23c 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/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 getLastReceivedTwist() - { - return *(received_velocity_msg_ptr_.readFromNonRT()); - } /** * @brief wait_for_twist block until a new twist is received. From 53de150f242e6879cc38906a832e8f189a891935 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 12:33:06 +0000 Subject: [PATCH 05/22] forward_command: Use ThreadSafeBox --- .../forward_controllers_base.hpp | 8 ++- .../src/forward_controllers_base.cpp | 58 ++++++++++++++---- .../test/test_forward_command_controller.cpp | 59 +++++++++---------- ...i_interface_forward_command_controller.cpp | 58 +++++++++--------- 4 files changed, 107 insertions(+), 76 deletions(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index c8b620d593..02d0d1a1c9 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,7 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller @@ -86,7 +86,11 @@ class ForwardControllersBase : public controller_interface::ControllerInterface std::vector command_interface_types_; - realtime_tools::RealtimeBuffer> rt_command_ptr_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType last_ref_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; }; diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 331d3f9eea..cb35acf2ea 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,6 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" +#include +#include #include #include #include @@ -23,12 +25,24 @@ #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg(forward_command_controller::CmdType & msg) +{ + for (auto & data : msg.data) + { + data = std::numeric_limits::quiet_NaN(); + } +} + +} // namespace + namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) +: controller_interface::ControllerInterface(), joints_command_subscriber_(nullptr) { } @@ -58,7 +72,7 @@ controller_interface::CallbackReturn ForwardControllersBase::on_configure( joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) { rt_command_.set(*msg); }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -101,7 +115,11 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( } // reset command buffer if a command came through callback when controller was inactive - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + CmdType empty_msg; + reset_controller_reference_msg(empty_msg); + rt_command_.try_set(empty_msg); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -110,34 +128,48 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // reset command buffer - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + CmdType empty_msg; + reset_controller_reference_msg(empty_msg); + rt_command_.try_set(empty_msg); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto joint_commands = rt_command_ptr_.readFromRT(); + auto joint_commands_op = rt_command_.try_get(); + CmdType joint_commands; + if (joint_commands_op.has_value()) + { + joint_commands = last_ref_ = joint_commands_op.value(); + } + else + { + joint_commands = last_ref_; + } // no command received yet - if (!joint_commands || !(*joint_commands)) + if (std::all_of( + joint_commands.data.cbegin(), joint_commands.data.cend(), + [](const auto & value) { return std::isnan(value); })) { return controller_interface::return_type::OK; } - if ((*joint_commands)->data.size() != command_interfaces_.size()) + if (joint_commands.data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*joint_commands)->data.size(), command_interfaces_.size()); + "command size (%zu) does not match number of interfaces (%zu)", joint_commands.data.size(), + command_interfaces_.size()); return controller_interface::return_type::ERROR; } for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value((*joint_commands)->data[index]); + command_interfaces_[index].set_value(joint_commands.data[index]); } return controller_interface::return_type::OK; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index bf20aa8eb6..3c936d8d0f 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -217,9 +218,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command_msg); // update successful, command received ASSERT_EQ( @@ -245,9 +246,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0}; + controller_->rt_command_.set(command_msg); // update failed, command size does not match number of joints ASSERT_EQ( @@ -362,10 +363,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) ASSERT_THAT(state_if_conf.names, IsEmpty()); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); - auto command_msg = std::make_shared(); - command_msg->data = {10.0, 20.0, 30.0}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + // send command + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful ASSERT_EQ( @@ -389,35 +390,31 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -430,7 +427,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7bd869677c..9407118598 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -15,6 +15,7 @@ /// \authors: Jack Center, Denis Stogl #include +#include #include #include #include @@ -199,9 +200,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) SetUpController(true); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -234,9 +235,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(true); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -294,9 +295,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -319,35 +320,32 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes ASSERT_THAT(state_if_conf.names, IsEmpty()); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); - // command ptr should be reset (nullptr) after deactivation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after deactivation - same check as in `update` + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - auto command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -360,7 +358,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( From 73ec7bb0c903668d481764790125bec8cb123f55 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 13:15:09 +0000 Subject: [PATCH 06/22] pid: Use ThreadSafeBox and std::atomic --- .../include/pid_controller/pid_controller.hpp | 11 ++- pid_controller/src/pid_controller.cpp | 91 ++++++++++++------- pid_controller/test/test_pid_controller.cpp | 72 +++++++-------- pid_controller/test/test_pid_controller.hpp | 17 ++-- .../test_pid_controller_dual_interface.cpp | 3 +- 5 files changed, 113 insertions(+), 81 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 34a50a3687..b3d830d96b 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -18,6 +18,7 @@ #ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ #define PID_CONTROLLER__PID_CONTROLLER_HPP_ +#include #include #include #include @@ -27,8 +28,8 @@ #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.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 "std_srvs/srv/set_bool.hpp" #include "pid_controller/pid_controller_parameters.hpp" @@ -82,13 +83,15 @@ class PidController : public controller_interface::ChainableControllerInterface // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeThreadSafeBox input_ref_; + ControllerReferenceMsg last_ref_; rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> measured_state_; + realtime_tools::RealtimeThreadSafeBox measured_state_; + ControllerMeasuredStateMsg last_state_; rclcpp::Service::SharedPtr set_feedforward_control_service_; - realtime_tools::RealtimeBuffer feedforward_mode_enabled_; + std::atomic feedforward_mode_enabled_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b1cb29e7c6..12660b3a65 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -53,15 +53,15 @@ using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceM // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & dof_names) + ControllerCommandMsg & msg, const std::vector & dof_names) { - msg->dof_names = dof_names; - msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); - msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg.dof_names = dof_names; + msg.values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg.values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); } void reset_controller_measured_state_msg( - const std::shared_ptr & msg, const std::vector & dof_names) + ControllerCommandMsg & msg, const std::vector & dof_names) { reset_controller_reference_msg(msg, dof_names); } @@ -74,7 +74,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf controller_interface::CallbackReturn PidController::on_init() { - feedforward_mode_enabled_.initRT(false); + feedforward_mode_enabled_ = false; try { @@ -97,7 +97,7 @@ void PidController::update_parameters() } params_ = param_listener_->get_params(); - feedforward_mode_enabled_.writeFromNonRT(params_.enable_feedforward); + feedforward_mode_enabled_ = params_.enable_feedforward; } controller_interface::CallbackReturn PidController::configure_parameters() @@ -179,9 +179,9 @@ controller_interface::CallbackReturn PidController::on_configure( "~/reference", subscribers_qos, std::bind(&PidController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg; reset_controller_reference_msg(msg, reference_and_state_dof_names_); - input_ref_.writeFromNonRT(msg); + input_ref_.set(msg); // input state Subscriber and callback if (params_.use_external_measured_states) @@ -227,16 +227,15 @@ controller_interface::CallbackReturn PidController::on_configure( } } // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(state_msg); + measured_state_.set(*state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); } - std::shared_ptr measured_state_msg = - std::make_shared(); + ControllerMeasuredStateMsg measured_state_msg; reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); - measured_state_.writeFromNonRT(measured_state_msg); + measured_state_.set(measured_state_msg); measured_state_values_.resize( dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); @@ -246,7 +245,7 @@ controller_interface::CallbackReturn PidController::on_configure( const std::shared_ptr request, std::shared_ptr response) { - feedforward_mode_enabled_.writeFromNonRT(request->data); + feedforward_mode_enabled_ = request->data; RCLCPP_WARN( get_node()->get_logger(), @@ -297,7 +296,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names = reference_and_state_dof_names_; - input_ref_.writeFromNonRT(ref_msg); + input_ref_.set(*ref_msg); } else if ( msg->dof_names.size() == reference_and_state_dof_names_.size() && @@ -306,7 +305,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.size(); ++i) @@ -329,7 +328,7 @@ void PidController::reference_callback(const std::shared_ptr::quiet_NaN()); @@ -453,21 +463,31 @@ controller_interface::CallbackReturn PidController::on_activate( controller_interface::return_type PidController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); + auto current_ref_op = input_ref_.try_get(); + ControllerReferenceMsg current_ref; + if (current_ref_op.has_value()) + { + current_ref = last_ref_ = current_ref_op.value(); + } + else + { + current_ref = last_ref_; + } for (size_t i = 0; i < dof_; ++i) { - if (!std::isnan((*current_ref)->values[i])) + if (!std::isnan(current_ref.values[i])) { - reference_interfaces_[i] = (*current_ref)->values[i]; - if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + reference_interfaces_[i] = current_ref.values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan(current_ref.values_dot[i])) { - reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + reference_interfaces_[dof_ + i] = current_ref.values_dot[i]; } - - (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + current_ref.values[i] = std::numeric_limits::quiet_NaN(); } } + // save cleared input_ref_ + input_ref_.try_set(current_ref); return controller_interface::return_type::OK; } @@ -480,13 +500,22 @@ controller_interface::return_type PidController::update_and_write_commands( // Update feedback either from external measured state or from state interfaces if (params_.use_external_measured_states) { - const auto measured_state = *(measured_state_.readFromRT()); + auto measured_state_op = measured_state_.try_get(); + ControllerMeasuredStateMsg measured_state; + if (measured_state_op.has_value()) + { + measured_state = last_state_ = measured_state_op.value(); + } + else + { + measured_state = last_state_; + } for (size_t i = 0; i < dof_; ++i) { - measured_state_values_[i] = measured_state->values[i]; + measured_state_values_[i] = measured_state.values[i]; if (measured_state_values_.size() == 2 * dof_) { - measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + measured_state_values_[dof_ + i] = measured_state.values_dot[i]; } } } @@ -512,7 +541,7 @@ controller_interface::return_type PidController::update_and_write_commands( if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward - if (*(feedforward_mode_enabled_.readFromRT())) + if (feedforward_mode_enabled_) { // two interfaces if (reference_interfaces_.size() == 2 * dof_) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 2a3b99e969..a0916ee249 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -131,14 +131,14 @@ TEST_F(PidControllerTest, 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_EQ((*msg)->values.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values) + auto msg = controller_->input_ref_.get(); + EXPECT_EQ(msg.values.size(), dof_names_.size()); + for (const auto & cmd : msg.values) { EXPECT_TRUE(std::isnan(cmd)); } - EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values_dot) + EXPECT_EQ(msg.values_dot.size(), dof_names_.size()); + for (const auto & cmd : msg.values_dot) { EXPECT_TRUE(std::isnan(cmd)); } @@ -203,21 +203,21 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) executor.add_node(service_caller_node_->get_node_base_interface()); // initially set to OFF - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // should stay false - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); // set to true ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->feedforward_mode_enabled_, true); // set back to false ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); } TEST_F(PidControllerTest, test_feedforward_mode_parameter) @@ -227,44 +227,44 @@ TEST_F(PidControllerTest, test_feedforward_mode_parameter) // Check updating mode during on_configure // initially set to OFF - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); // Reconfigure after setting parameter to true ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->feedforward_mode_enabled_, true); ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); // initially set to ON - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->feedforward_mode_enabled_, true); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); // Check updating mode during update_and_write_commands ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); // Switch to ON ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->feedforward_mode_enabled_, true); // Switch to OFF EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); } /** @@ -283,8 +283,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) controller_->set_chained_mode(false); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0])); + EXPECT_EQ(controller_->feedforward_mode_enabled_, false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -294,8 +294,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().values[i])); + EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); } @@ -303,13 +303,13 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + EXPECT_EQ(controller_->feedforward_mode_enabled_, false); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[i])); } // check the command value // error = ref - state = 100.001, error_dot = error/ds = 10000.1, @@ -337,8 +337,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->set_chained_mode(false); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0])); + EXPECT_EQ(controller_->feedforward_mode_enabled_, false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -346,13 +346,13 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->set_reference(dof_command_values_); - controller_->feedforward_mode_enabled_.writeFromNonRT(true); - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + controller_->feedforward_mode_enabled_ = true; + EXPECT_EQ(controller_->feedforward_mode_enabled_, true); for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().values[i])); + EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); } @@ -360,13 +360,13 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + EXPECT_EQ(controller_->feedforward_mode_enabled_, true); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); for (size_t i = 0; i < dof_command_values_.size(); ++i) { - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[i])); // check the command value: // ref = 101.101, state = 1.1, ds = 0.01 @@ -400,7 +400,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward mode is off as default, use this for convenience - EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + EXPECT_EQ(controller_->feedforward_mode_enabled_, false); // update reference interface which will be used for calculation const double ref_interface_value = 5.0; @@ -612,8 +612,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->feedforward_mode_enabled_.writeFromNonRT(true); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + controller_->feedforward_mode_enabled_ = true; + ASSERT_EQ(controller_->feedforward_mode_enabled_, true); // send a message to update reference interface controller_->set_reference({target_value}); @@ -671,7 +671,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward by default is OFF - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->feedforward_mode_enabled_, false); // send a message to update reference interface controller_->set_reference({target_value}); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1eb55ec878..c5f528cd79 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -109,15 +109,15 @@ class TestablePidController : public pid_controller::PidController void set_reference(const std::vector & target_value) { - std::shared_ptr msg = std::make_shared(); - msg->dof_names = params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) + ControllerCommandMsg msg; + msg.dof_names = params_.dof_names; + msg.values.resize(msg.dof_names.size(), 0.0); + for (size_t i = 0; i < msg.dof_names.size(); ++i) { - msg->values[i] = target_value[i]; + msg.values[i] = target_value[i]; } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - input_ref_.writeFromNonRT(msg); + msg.values_dot.resize(msg.dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.set(msg); } }; @@ -140,7 +140,8 @@ class PidControllerFixture : public ::testing::Test // initialize controller controller_ = std::make_unique(); - command_publisher_node_ = std::make_shared("command_publisher"); + auto test = std::make_shared("command_publisher"); + command_publisher_node_ = test; command_publisher_ = command_publisher_node_->create_publisher( "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 6db9171bcb..ad13c73a0c 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -78,8 +78,7 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i ASSERT_TRUE(controller_->is_in_chained_mode()); // turn on feedforward - controller_->feedforward_mode_enabled_.writeFromNonRT(true); - ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + controller_->feedforward_mode_enabled_ = true; // set up the reference interface, controller_->reference_interfaces_ = { From 36526a94c97d18cc616d685f65e79e1ae1b36480 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 17:50:47 +0000 Subject: [PATCH 07/22] gpio: Use ThreadSafeBox --- .../gpio_command_controller.hpp | 8 +++- .../src/gpio_command_controller.cpp | 46 ++++++++++++++++--- .../test/test_gpio_command_controller.cpp | 14 +++--- 3 files changed, 51 insertions(+), 17 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 35cf7c283d..c4f2a7a609 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -24,8 +24,8 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.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 "gpio_controllers/gpio_command_controller_parameters.hpp" @@ -91,9 +91,13 @@ class GpioCommandController : public controller_interface::ControllerInterface MapOfReferencesToCommandInterfaces command_interfaces_map_; MapOfReferencesToStateInterfaces state_interfaces_map_; - realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType last_ref_; + std::shared_ptr> gpio_state_publisher_{}; std::shared_ptr> realtime_gpio_state_publisher_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fdecf71c30..6cf4bb561c 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -23,7 +23,7 @@ #include "rclcpp/subscription.hpp" namespace -{ +{ // utility template void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) { @@ -33,6 +33,15 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces } } +// called from RT control loop +void reset_controller_reference_msg( + gpio_controllers::CmdType & msg, const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.interface_groups.clear(); + msg.interface_values.clear(); +} + std::vector extract_gpios_from_hardware_info( const std::vector & hardware_infos) { @@ -85,7 +94,7 @@ try { gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) { rt_command_.set(*msg); }); } gpio_state_publisher_ = @@ -137,14 +146,28 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State } initialize_gpio_state_msg(); - rt_command_ptr_.reset(); + // Set default value in command (the same number as state interfaces) + auto rt_command_op = rt_command_.try_get(); + if (rt_command_op.has_value()) + { + auto rt_command = rt_command_op.value(); + reset_controller_reference_msg(rt_command, get_node()); + rt_command_.try_set(rt_command); + } RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { - rt_command_ptr_.reset(); + // Set default value in command (the same number as state interfaces) + auto rt_command_op = rt_command_.try_get(); + if (rt_command_op.has_value()) + { + auto rt_command = rt_command_op.value(); + reset_controller_reference_msg(rt_command, get_node()); + rt_command_.try_set(rt_command); + } return CallbackReturn::SUCCESS; } @@ -315,13 +338,22 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( controller_interface::return_type GpioCommandController::update_gpios_commands() { - auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); - if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + auto gpio_commands_op = rt_command_.try_get(); + CmdType gpio_commands; + if (gpio_commands_op.has_value()) + { + gpio_commands = last_ref_ = gpio_commands_op.value(); + } + else + { + gpio_commands = last_ref_; + } + if (gpio_commands.interface_groups.empty() || gpio_commands.interface_values.empty()) { + // no command received yet return controller_interface::return_type::OK; } - const auto gpio_commands = *(*gpio_commands_ptr); for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) { const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index de085894f2..c252ed4bf9 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -445,7 +445,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -467,7 +467,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -489,7 +489,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -513,7 +513,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -536,8 +536,7 @@ TEST_F( const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); - - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -561,8 +560,7 @@ TEST_F( const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), createInterfaceValue({"ana.1"}, {21.0})}); - - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(command); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); From 478c9ca677f7e2e9dc9418cf0881cdb003ec0e22 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 18:09:53 +0000 Subject: [PATCH 08/22] parallelgripper: Use ThreadSafeBox --- .../parallel_gripper_action_controller.hpp | 20 +++++----- ...arallel_gripper_action_controller_impl.hpp | 37 ++++++++++++------- 2 files changed, 34 insertions(+), 23 deletions(-) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 0bd48b65f5..93a6f6ec65 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -36,8 +36,8 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // Project #include "parallel_gripper_controller/parallel_gripper_action_controller_parameters.hpp" @@ -93,10 +93,6 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is reused to set the realtime buffer - Commands command_struct_, command_struct_rt_; - protected: using GripperCommandAction = control_msgs::action::ParallelGripperCommand; using ActionServer = rclcpp_action::Server; @@ -105,12 +101,16 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBox = realtime_tools::RealtimeThreadSafeBox; - bool update_hold_position_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox command_; + // pre-allocated memory that is reused + Commands command_struct_; + // save the last reference in case of unable to get value from box + Commands command_struct_rt_; - bool verbose_ = false; ///< Hard coded verbose flag to help in debugging - std::string name_; ///< Controller name. + std::string name_; ///< Controller name. std::optional> joint_command_interface_; std::optional> @@ -125,7 +125,7 @@ class GripperActionController : public controller_interface::ControllerInterface std::shared_ptr param_listener_; Params params_; - RealtimeGoalHandleBuffer + RealtimeGoalHandleBox rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 50f91678cb..87bd2c34a5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -30,12 +30,14 @@ namespace parallel_gripper_action_controller void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal) { // Marks the current goal as canceled active_goal->setCanceled(std::make_shared()); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } @@ -57,7 +59,11 @@ controller_interface::CallbackReturn GripperActionController::on_init() controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - command_struct_rt_ = *(command_.readFromRT()); + auto command_struct_rt_op = command_.try_get(); + if (command_struct_rt_op.has_value()) + { + command_struct_rt_ = command_struct_rt_op.value(); + } const double current_position = joint_position_state_interface_->get().get_value(); const double current_velocity = joint_velocity_state_interface_->get().get_value(); @@ -124,14 +130,14 @@ void GripperActionController::accepted_callback( { command_struct_.max_effort_ = params_.max_effort; } - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; last_movement_time_ = get_node()->now(); rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); + rt_active_goal_.set([rt_goal](RealtimeGoalHandlePtr & stored_value) { stored_value = rt_goal; }); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -148,7 +154,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode @@ -161,7 +168,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( auto action_res = std::make_shared(); active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -171,14 +179,15 @@ void GripperActionController::set_hold_position() command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); } void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (!active_goal) { return; @@ -192,7 +201,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); active_goal->setSucceeded(pre_alloc_result_); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } else { @@ -217,7 +227,8 @@ void GripperActionController::check_for_success( RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } } @@ -316,11 +327,11 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - non RT version + // Command command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.initRT(command_struct_); + command_.try_set(command_struct_); // Result pre_alloc_result_ = std::make_shared(); From 59e334f90c4a43338e7e40d2b4a35fdb1d847cce Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:28:34 +0000 Subject: [PATCH 09/22] admittance: Use ThreadSafeBox --- .../admittance_controller.hpp | 15 ++--- .../src/admittance_controller.cpp | 57 +++++++++++++++---- 2 files changed, 54 insertions(+), 18 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 23aad881c6..47c9fd514a 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/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 parameter_handler_; - // ROS messages - std::shared_ptr joint_command_msg_; - - // real-time buffer - realtime_tools::RealtimeBuffer> + // real-time boxes + realtime_tools::RealtimeThreadSafeBox input_joint_command_; - realtime_tools::RealtimeBuffer input_wrench_command_; + realtime_tools::RealtimeThreadSafeBox input_wrench_command_; std::unique_ptr> 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_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5c652231fd..4f915d204c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/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 & 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::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 msg) - { input_joint_command_.writeFromNonRT(msg); }; + { input_joint_command_.set(*msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( "~/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( "~/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; } From 0d28d35e708744e822b02c485898b1bc55d9cd43 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Jun 2025 19:52:05 +0000 Subject: [PATCH 10/22] Update rt box of steering_controller_library --- .../steering_controllers_library.hpp | 4 +- .../src/steering_controllers_library.cpp | 90 ++++++--- .../test_steering_controllers_library.cpp | 177 +++++++++++++----- .../test_steering_controllers_library.hpp | 3 +- 4 files changed, 195 insertions(+), 79 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e700767e4c..ba00a9c629 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.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" // TODO(anyone): Replace with controller specific messages #include "control_msgs/msg/steering_controller_status.hpp" @@ -83,7 +83,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeThreadSafeBox input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f34b3a78e0..7e0a38733d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -31,16 +31,15 @@ using ControllerTwistReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerTwistReferenceMsg & msg, const std::shared_ptr & node) { - msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); + msg.header.stamp = node->now(); + msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } } // namespace @@ -333,10 +332,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = - std::make_shared(); + ControllerTwistReferenceMsg msg; reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + input_ref_.set(msg); try { @@ -433,7 +431,7 @@ void SteeringControllersLibrary::reference_callback( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_.writeFromNonRT(msg); + input_ref_.set(*msg); } else { @@ -518,8 +516,11 @@ bool SteeringControllersLibrary::on_set_chained_mode(bool /*chained_mode*/) { re controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + ControllerTwistReferenceMsg emtpy_msg; + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.try_set(emtpy_msg); return controller_interface::CallbackReturn::SUCCESS; } @@ -535,14 +536,46 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( } controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); + auto current_ref_op = input_ref_.try_get(); + if (!current_ref_op.has_value()) + { + // reference_interfaces_ will remain unchanged + return controller_interface::return_type::OK; + } + auto current_ref = current_ref_op.value(); + const auto age_of_last_command = time - current_ref.header.stamp; - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + // accept message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y)) + { + reference_interfaces_[0] = current_ref.twist.linear.x; + reference_interfaces_[1] = current_ref.twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref); + } + } + } + else + { + if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.angular.z)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref); + } } return controller_interface::return_type::OK; @@ -560,27 +593,26 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; - const auto timeout = - age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); - - // store (for open loop odometry) and set commands - last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; - last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; - auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + command_interfaces_[i].set_value(traction_commands[i]); } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { command_interfaces_[i + params_.traction_joints_names.size()].set_value(steering_commands[i]); } } + else + { + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + command_interfaces_[i].set_value(0.0); + } + } // Publish odometry message // Compute and store orientation info diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 26750777c9..a88b84874c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -80,11 +80,12 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) } // Tests controller update_reference_from_subscribers and -// its two cases for position_feedback true/false behavior +// for position_feedback behavior // when too old msg is sent i.e age_of_last_command > ref_timeout case -TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) { SetUpController(); + controller_->params_.position_feedback = true; rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -104,46 +105,76 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg; - // adjusting to achieve age_of_last_command > ref_timeout - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // case 1 position_feedback = false - controller_->params_.position_feedback = false; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + // age_of_last_command < ref_timeout_ + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); @@ -152,42 +183,94 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // Steer angles should not reset EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); +} - // case 2 position_feedback = true - controller_->params_.position_feedback = true; +// Tests controller update_reference_from_subscribers and +// for position_feedback=false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) +{ + SetUpController(); + controller_->params_.position_feedback = false; - // adjusting to achieve age_of_last_command > ref_timeout - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; + + ControllerReferenceMsg msg; + + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // age_of_last_command < ref_timeout_ + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // adjusting to achieve age_of_last_command > ref_timeout + msg.header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg.twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg.twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // Wheel velocities should reset to 0 EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 22d70fd7a5..5aac580b33 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -70,7 +70,8 @@ class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); - FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout); public: controller_interface::CallbackReturn on_configure( From b6b617a70b381b5205adb01bcf01bfa515bd3fe6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 11:54:35 +0000 Subject: [PATCH 11/22] steering: save last_ref_ --- .../steering_controllers_library.hpp | 7 +++++-- .../src/steering_controllers_library.cpp | 16 ++++++++++++---- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index ba00a9c629..c083640372 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -81,11 +81,14 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::shared_ptr param_listener_; steering_controllers_library::Params params_; - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + // the RT Box containing the command message realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerTwistReferenceMsg last_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 7e0a38733d..a87510def3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -539,17 +539,22 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref_op = input_ref_.try_get(); - if (!current_ref_op.has_value()) + ControllerTwistReferenceMsg current_ref; + if (current_ref_op.has_value()) { - // reference_interfaces_ will remain unchanged - return controller_interface::return_type::OK; + current_ref = last_ref_ = current_ref_op.value(); } - auto current_ref = current_ref_op.value(); + else + { + current_ref = last_ref_; + } + const auto age_of_last_command = time - current_ref.header.stamp; // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { + std::cout << "NO TIMEOUT" << std::endl; if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y)) { reference_interfaces_[0] = current_ref.twist.linear.x; @@ -566,6 +571,7 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { + std::cout << "TIMEOUT" << std::endl; if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.angular.z)) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); @@ -593,6 +599,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { + std::cout << "Update commands" << std::endl; auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); @@ -608,6 +615,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { + std::cout << "Update commands with zero vel" << std::endl; for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { command_interfaces_[i].set_value(0.0); From 13c0f7677355270503be9fa1945f7b337b049d47 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 11:55:07 +0000 Subject: [PATCH 12/22] Fix steering controllers tests --- .../test_ackermann_steering_controller.cpp | 28 +++++++++---------- .../test/test_bicycle_steering_controller.cpp | 28 +++++++++---------- .../test_tricycle_steering_controller.cpp | 28 +++++++++---------- 3 files changed, 42 insertions(+), 42 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index f7cdca092f..5393f779a1 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/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 msg = std::make_shared(); - 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_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_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_) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 23103f7974..b8916d55d0 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/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 msg = std::make_shared(); - 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)), @@ -163,7 +163,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_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_) { @@ -195,7 +195,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_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_) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 4f9b53e31f..be998de998 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -96,13 +96,13 @@ TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, update_success) @@ -154,11 +154,11 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - std::shared_ptr msg = std::make_shared(); - 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)), @@ -174,7 +174,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_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_) { @@ -211,7 +211,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_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_) { From 244c044953e5eb515f1dad00c2da6e0de1c9efeb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:35:08 +0000 Subject: [PATCH 13/22] mecanum: preallocate --- .../mecanum_drive_controller.hpp | 2 +- .../src/mecanum_drive_controller.cpp | 42 +++++++++---------- 2 files changed, 21 insertions(+), 23 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 221466eaf0..c0edc8c5a2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -112,7 +112,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // the RT Box containing the command message realtime_tools::RealtimeThreadSafeBox input_ref_; // save the last reference in case of unable to get value from box - ControllerReferenceMsg last_ref_; + ControllerReferenceMsg current_ref_; // the reference timeout value from parameters rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 1e07b63492..fd747edb03 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -128,9 +128,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - ControllerReferenceMsg msg; - reset_controller_reference_msg(msg, get_node()); - input_ref_.set(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -338,54 +337,53 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref_op = input_ref_.try_get(); - ControllerReferenceMsg current_ref; if (current_ref_op.has_value()) { - current_ref = last_ref_ = current_ref_op.value(); + current_ref_ = current_ref_op.value(); } else { - current_ref = last_ref_; + current_ref_; } - const auto age_of_last_command = time - current_ref.header.stamp; + const auto age_of_last_command = time - current_ref_.header.stamp; // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { if ( - !std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y) && - !std::isnan(current_ref.twist.angular.z)) + !std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y) && + !std::isnan(current_ref_.twist.angular.z)) { - reference_interfaces_[0] = current_ref.twist.linear.x; - reference_interfaces_[1] = current_ref.twist.linear.y; - reference_interfaces_[2] = current_ref.twist.angular.z; + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.linear.y; + reference_interfaces_[2] = current_ref_.twist.angular.z; if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref.twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set(current_ref); + input_ref_.try_set(current_ref_); } } } else { if ( - !std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y) && - !std::isnan(current_ref.twist.angular.z)) + !std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y) && + !std::isnan(current_ref_.twist.angular.z)) { reference_interfaces_[0] = 0.0; reference_interfaces_[1] = 0.0; reference_interfaces_[2] = 0.0; - current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref.twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set(current_ref); + input_ref_.try_set(current_ref_); } } From e9712700c09f656d0fe499668883714034d4531c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:38:10 +0000 Subject: [PATCH 14/22] diffdrive: preallocate --- .../diff_drive_controller.hpp | 2 +- .../src/diff_drive_controller.cpp | 31 +++++++++---------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index d7d775de83..0ca1d023ec 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -125,7 +125,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte // the realtime container to exchange the reference from subscriber realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; // save the last reference in case of unable to get value from box - TwistStamped last_ref_; + TwistStamped command_msg_; std::queue> previous_two_commands_; // speed limiters diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index da444a7b3e..8bb98bf2ab 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -108,27 +108,27 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub auto logger = get_node()->get_logger(); auto current_ref_op = received_velocity_msg_.try_get(); - TwistStamped command_msg; if (current_ref_op.has_value()) { - command_msg = last_ref_ = current_ref_op.value(); + command_msg_ = current_ref_op.value(); } else { - command_msg = last_ref_; + command_msg_; } - const auto age_of_last_command = time - command_msg.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.twist.linear.x) && std::isfinite(command_msg.twist.angular.z)) + else if ( + std::isfinite(command_msg_.twist.linear.x) && std::isfinite(command_msg_.twist.angular.z)) { - reference_interfaces_[0] = command_msg.twist.linear.x; - reference_interfaces_[1] = command_msg.twist.angular.z; + reference_interfaces_[0] = command_msg_.twist.linear.x; + reference_interfaces_[1] = command_msg_.twist.angular.z; } else { @@ -570,15 +570,14 @@ void DiffDriveController::reset_buffers() // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - TwistStamped empty_msg; - empty_msg.header.stamp = get_node()->now(); - empty_msg.twist.linear.x = std::numeric_limits::quiet_NaN(); - empty_msg.twist.linear.y = std::numeric_limits::quiet_NaN(); - empty_msg.twist.linear.z = std::numeric_limits::quiet_NaN(); - empty_msg.twist.angular.x = std::numeric_limits::quiet_NaN(); - empty_msg.twist.angular.y = std::numeric_limits::quiet_NaN(); - empty_msg.twist.angular.z = std::numeric_limits::quiet_NaN(); - received_velocity_msg_.set(empty_msg); + command_msg_.header.stamp = get_node()->now(); + command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(command_msg_); } void DiffDriveController::halt() From 3cb97e021f283db75d4fa553f5d6cb6aa2184539 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:41:52 +0000 Subject: [PATCH 15/22] forward: preallocate --- .../forward_controllers_base.hpp | 2 +- .../src/forward_controllers_base.cpp | 23 ++++++++----------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 02d0d1a1c9..9f03042cdf 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -89,7 +89,7 @@ class ForwardControllersBase : public controller_interface::ControllerInterface // the realtime container to exchange the reference from subscriber realtime_tools::RealtimeThreadSafeBox rt_command_; // save the last reference in case of unable to get value from box - CmdType last_ref_; + CmdType joint_commands_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; }; diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index cb35acf2ea..514a074553 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -117,9 +117,8 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( // reset command buffer if a command came through callback when controller was inactive // Try to set default value in command. // If this fails, then another command will be received soon anyways. - CmdType empty_msg; - reset_controller_reference_msg(empty_msg); - rt_command_.try_set(empty_msg); + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -129,9 +128,8 @@ controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // Try to set default value in command. - CmdType empty_msg; - reset_controller_reference_msg(empty_msg); - rt_command_.try_set(empty_msg); + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); return controller_interface::CallbackReturn::SUCCESS; } @@ -140,36 +138,35 @@ controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto joint_commands_op = rt_command_.try_get(); - CmdType joint_commands; if (joint_commands_op.has_value()) { - joint_commands = last_ref_ = joint_commands_op.value(); + joint_commands_ = joint_commands_op.value(); } else { - joint_commands = last_ref_; + joint_commands_; } // no command received yet if (std::all_of( - joint_commands.data.cbegin(), joint_commands.data.cend(), + joint_commands_.data.cbegin(), joint_commands_.data.cend(), [](const auto & value) { return std::isnan(value); })) { return controller_interface::return_type::OK; } - if (joint_commands.data.size() != command_interfaces_.size()) + if (joint_commands_.data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", joint_commands.data.size(), + "command size (%zu) does not match number of interfaces (%zu)", joint_commands_.data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; } for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value(joint_commands.data[index]); + command_interfaces_[index].set_value(joint_commands_.data[index]); } return controller_interface::return_type::OK; From ea6379ca50f85912a8afb6ba4434bdd5a7ec8814 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:48:19 +0000 Subject: [PATCH 16/22] pid: preallocate --- .../include/pid_controller/pid_controller.hpp | 4 +- pid_controller/src/pid_controller.cpp | 46 +++++++------------ 2 files changed, 19 insertions(+), 31 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index b3d830d96b..c5b1ea58c4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -84,11 +84,11 @@ class PidController : public controller_interface::ChainableControllerInterface // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeThreadSafeBox input_ref_; - ControllerReferenceMsg last_ref_; + ControllerReferenceMsg current_ref_; rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; realtime_tools::RealtimeThreadSafeBox measured_state_; - ControllerMeasuredStateMsg last_state_; + ControllerMeasuredStateMsg current_state_; rclcpp::Service::SharedPtr set_feedforward_control_service_; std::atomic feedforward_mode_enabled_; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 12660b3a65..5a1bd1d977 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -179,9 +179,8 @@ controller_interface::CallbackReturn PidController::on_configure( "~/reference", subscribers_qos, std::bind(&PidController::reference_callback, this, std::placeholders::_1)); - ControllerReferenceMsg msg; - reset_controller_reference_msg(msg, reference_and_state_dof_names_); - input_ref_.set(msg); + reset_controller_reference_msg(current_ref_, reference_and_state_dof_names_); + input_ref_.set(current_ref_); // input state Subscriber and callback if (params_.use_external_measured_states) @@ -435,16 +434,15 @@ controller_interface::CallbackReturn PidController::on_activate( auto input_ref_op = input_ref_.try_get(); if (input_ref_op.has_value()) { - auto input_ref = input_ref_op.value(); - reset_controller_reference_msg(input_ref, reference_and_state_dof_names_); - input_ref_.try_set(input_ref); + current_ref_ = input_ref_op.value(); + reset_controller_reference_msg(current_ref_, reference_and_state_dof_names_); + input_ref_.try_set(current_ref_); } auto measured_state_op = measured_state_.try_get(); if (measured_state_op.has_value()) { - auto measured_state = measured_state_op.value(); - reset_controller_measured_state_msg(measured_state, reference_and_state_dof_names_); - measured_state_.try_set(measured_state); + reset_controller_measured_state_msg(current_state_, reference_and_state_dof_names_); + measured_state_.try_set(current_state_); } reference_interfaces_.assign( @@ -464,30 +462,25 @@ controller_interface::return_type PidController::update_reference_from_subscribe const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto current_ref_op = input_ref_.try_get(); - ControllerReferenceMsg current_ref; if (current_ref_op.has_value()) { - current_ref = last_ref_ = current_ref_op.value(); - } - else - { - current_ref = last_ref_; + current_ref_ = current_ref_op.value(); } for (size_t i = 0; i < dof_; ++i) { - if (!std::isnan(current_ref.values[i])) + if (!std::isnan(current_ref_.values[i])) { - reference_interfaces_[i] = current_ref.values[i]; - if (reference_interfaces_.size() == 2 * dof_ && !std::isnan(current_ref.values_dot[i])) + reference_interfaces_[i] = current_ref_.values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan(current_ref_.values_dot[i])) { - reference_interfaces_[dof_ + i] = current_ref.values_dot[i]; + reference_interfaces_[dof_ + i] = current_ref_.values_dot[i]; } - current_ref.values[i] = std::numeric_limits::quiet_NaN(); + current_ref_.values[i] = std::numeric_limits::quiet_NaN(); } } // save cleared input_ref_ - input_ref_.try_set(current_ref); + input_ref_.try_set(current_ref_); return controller_interface::return_type::OK; } @@ -501,21 +494,16 @@ controller_interface::return_type PidController::update_and_write_commands( if (params_.use_external_measured_states) { auto measured_state_op = measured_state_.try_get(); - ControllerMeasuredStateMsg measured_state; if (measured_state_op.has_value()) { - measured_state = last_state_ = measured_state_op.value(); - } - else - { - measured_state = last_state_; + current_state_ = measured_state_op.value(); } for (size_t i = 0; i < dof_; ++i) { - measured_state_values_[i] = measured_state.values[i]; + measured_state_values_[i] = current_state_.values[i]; if (measured_state_values_.size() == 2 * dof_) { - measured_state_values_[dof_ + i] = measured_state.values_dot[i]; + measured_state_values_[dof_ + i] = current_state_.values_dot[i]; } } } From 398971fe8bff88af5a6a3fc21b9a2cb18ef93188 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:48:27 +0000 Subject: [PATCH 17/22] Cleanup --- diff_drive_controller/src/diff_drive_controller.cpp | 4 ---- forward_command_controller/src/forward_controllers_base.cpp | 4 ---- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 ---- 3 files changed, 12 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8bb98bf2ab..2742c39695 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -112,10 +112,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub { command_msg_ = current_ref_op.value(); } - else - { - command_msg_; - } const auto age_of_last_command = time - command_msg_.header.stamp; // Brake if cmd_vel has timeout, override the stored command diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 514a074553..6bd90c8152 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -142,10 +142,6 @@ controller_interface::return_type ForwardControllersBase::update( { joint_commands_ = joint_commands_op.value(); } - else - { - joint_commands_; - } // no command received yet if (std::all_of( diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index fd747edb03..e69f075234 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -341,10 +341,6 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ { current_ref_ = current_ref_op.value(); } - else - { - current_ref_; - } const auto age_of_last_command = time - current_ref_.header.stamp; From aae8f00c27e6e807254aadec9d0a04aaf5de9731 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 19:54:43 +0000 Subject: [PATCH 18/22] gpio: preallocate --- .../gpio_command_controller.hpp | 2 +- .../src/gpio_command_controller.cpp | 44 +++++++------------ 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index c4f2a7a609..f7d75e1c20 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -96,7 +96,7 @@ class GpioCommandController : public controller_interface::ControllerInterface // the realtime container to exchange the reference from subscriber realtime_tools::RealtimeThreadSafeBox rt_command_; // save the last reference in case of unable to get value from box - CmdType last_ref_; + CmdType gpio_commands_; std::shared_ptr> gpio_state_publisher_{}; std::shared_ptr> realtime_gpio_state_publisher_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 6cf4bb561c..bdabbb3f13 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -146,28 +146,18 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State } initialize_gpio_state_msg(); - // Set default value in command (the same number as state interfaces) - auto rt_command_op = rt_command_.try_get(); - if (rt_command_op.has_value()) - { - auto rt_command = rt_command_op.value(); - reset_controller_reference_msg(rt_command, get_node()); - rt_command_.try_set(rt_command); - } + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { - // Set default value in command (the same number as state interfaces) - auto rt_command_op = rt_command_.try_get(); - if (rt_command_op.has_value()) - { - auto rt_command = rt_command_op.value(); - reset_controller_reference_msg(rt_command, get_node()); - rt_command_.try_set(rt_command); - } + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); return CallbackReturn::SUCCESS; } @@ -339,27 +329,23 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( controller_interface::return_type GpioCommandController::update_gpios_commands() { auto gpio_commands_op = rt_command_.try_get(); - CmdType gpio_commands; if (gpio_commands_op.has_value()) { - gpio_commands = last_ref_ = gpio_commands_op.value(); + gpio_commands_ = gpio_commands_op.value(); } - else - { - gpio_commands = last_ref_; - } - if (gpio_commands.interface_groups.empty() || gpio_commands.interface_values.empty()) + if (gpio_commands_.interface_groups.empty() || gpio_commands_.interface_values.empty()) { // no command received yet return controller_interface::return_type::OK; } - for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + for (std::size_t gpio_index = 0; gpio_index < gpio_commands_.interface_groups.size(); + ++gpio_index) { - const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + const auto & gpio_name = gpio_commands_.interface_groups[gpio_index]; if ( - gpio_commands.interface_values[gpio_index].values.size() != - gpio_commands.interface_values[gpio_index].interface_names.size()) + gpio_commands_.interface_values[gpio_index].values.size() != + gpio_commands_.interface_values[gpio_index].interface_names.size()) { RCLCPP_ERROR( get_node()->get_logger(), "For gpio %s interfaces_names do not match values", @@ -367,10 +353,10 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() return controller_interface::return_type::ERROR; } for (std::size_t command_interface_index = 0; - command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + command_interface_index < gpio_commands_.interface_values[gpio_index].values.size(); ++command_interface_index) { - apply_command(gpio_commands, gpio_index, command_interface_index); + apply_command(gpio_commands_, gpio_index, command_interface_index); } } return controller_interface::return_type::OK; From e41aafe1a2b90ab74f2f6635e069d0095f05c6e2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 20:11:33 +0000 Subject: [PATCH 19/22] steering: fix tests --- .../test/test_ackermann_steering_controller.cpp | 5 +++-- .../test/test_bicycle_steering_controller.cpp | 3 ++- .../test/test_tricycle_steering_controller.cpp | 5 +++-- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 5393f779a1..b151dac048 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -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); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index b8916d55d0..0db15e87e2 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -234,7 +234,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 --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index be998de998..ccc060f4c0 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -235,8 +235,9 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu 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); publish_commands(); From ba875d37d60d2e9585aa2f91ae21d86ea2e46402 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 20:11:55 +0000 Subject: [PATCH 20/22] steering: preallocate --- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 39 ++++++++----------- 2 files changed, 17 insertions(+), 24 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c083640372..83057652a5 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -84,7 +84,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // the RT Box containing the command message realtime_tools::RealtimeThreadSafeBox input_ref_; // save the last reference in case of unable to get value from box - ControllerTwistReferenceMsg last_ref_; + ControllerTwistReferenceMsg current_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms // Command subscribers and Controller State publisher diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a87510def3..8082429cee 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -332,9 +332,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - ControllerTwistReferenceMsg msg; - reset_controller_reference_msg(msg, get_node()); - input_ref_.set(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -518,9 +517,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( { // Try to set default value in command. // If this fails, then another command will be received soon anyways. - ControllerTwistReferenceMsg emtpy_msg; - reset_controller_reference_msg(emtpy_msg, get_node()); - input_ref_.try_set(emtpy_msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.try_set(current_ref_); return controller_interface::CallbackReturn::SUCCESS; } @@ -539,48 +537,43 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref_op = input_ref_.try_get(); - ControllerTwistReferenceMsg current_ref; if (current_ref_op.has_value()) { - current_ref = last_ref_ = current_ref_op.value(); - } - else - { - current_ref = last_ref_; + current_ref_ = current_ref_op.value(); } - const auto age_of_last_command = time - current_ref.header.stamp; + const auto age_of_last_command = time - current_ref_.header.stamp; // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { std::cout << "NO TIMEOUT" << std::endl; - if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.linear.y)) + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) { - reference_interfaces_[0] = current_ref.twist.linear.x; - reference_interfaces_[1] = current_ref.twist.angular.z; + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.angular.z; if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set(current_ref); + input_ref_.try_set(current_ref_); } } } else { std::cout << "TIMEOUT" << std::endl; - if (!std::isnan(current_ref.twist.linear.x) && !std::isnan(current_ref.twist.angular.z)) + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - current_ref.twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref.twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); - input_ref_.try_set(current_ref); + input_ref_.try_set(current_ref_); } } From 8b4e0d17df0069751b30e9565613ba1065625ad0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Jun 2025 20:17:43 +0000 Subject: [PATCH 21/22] steering: Remove debug output --- .../src/steering_controllers_library.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 8082429cee..510b265b3e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -547,7 +547,6 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f // accept message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - std::cout << "NO TIMEOUT" << std::endl; if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) { reference_interfaces_[0] = current_ref_.twist.linear.x; @@ -564,7 +563,6 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { - std::cout << "TIMEOUT" << std::endl; if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); @@ -592,7 +590,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - std::cout << "Update commands" << std::endl; auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); @@ -608,7 +605,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - std::cout << "Update commands with zero vel" << std::endl; for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { command_interfaces_[i].set_value(0.0); From da058ca958454f7ecc1f30935e2dcb4e8477b822 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 4 Jun 2025 07:03:31 +0000 Subject: [PATCH 22/22] Fix the forward controller specializations --- .../test/test_joint_group_effort_controller.cpp | 12 ++++++------ .../test/test_joint_group_position_controller.cpp | 12 ++++++------ .../test/test_joint_group_velocity_controller.cpp | 12 ++++++------ 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 01125ac442..dda7a755db 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index ea7005c543..7d575e7d6a 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 9a9af3fb81..fd7dec4cb2 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -111,9 +111,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -133,9 +133,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ(