From 07ddb359f7670605a24ce7c5becb42f53992c5a8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Nov 2024 22:46:23 +0000 Subject: [PATCH 1/6] Fix diff_drive_controller --- .../src/diff_drive_controller.cpp | 14 ++++++++------ .../test/test_diff_drive_controller.cpp | 3 ++- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 66da6d6738..26e2e1f4d3 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -112,7 +112,8 @@ controller_interface::return_type DiffDriveController::update( } std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) + { last_command_msg = msg; }); if (last_command_msg == nullptr) { @@ -325,8 +326,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } const Twist empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) + { stored_value = std::make_shared(empty_twist); }); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); @@ -349,7 +350,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -475,7 +477,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } @@ -502,7 +503,8 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); + received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) + { stored_value = nullptr; }); is_halted = false; return true; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..e29c6bbfee 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } From 90a3dc7361787415b90546721f7de44d1907dfd9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Nov 2024 22:53:33 +0000 Subject: [PATCH 2/6] Rename Twist->TwistStamped --- .../diff_drive_controller.hpp | 14 ++++----- .../src/diff_drive_controller.cpp | 29 ++++++++++--------- 2 files changed, 22 insertions(+), 21 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 ac34b81eca..74526b199c 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 @@ -45,7 +45,7 @@ namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; public: DIFF_DRIVE_CONTROLLER_PUBLIC @@ -128,20 +128,20 @@ class DiffDriveController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> + realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 26e2e1f4d3..67eadca2a1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,8 +111,8 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) { last_command_msg = msg; }); if (last_command_msg == nullptr) @@ -131,7 +131,7 @@ controller_interface::return_type DiffDriveController::update( // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; + TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -319,23 +319,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( if (publish_limited_velocity_) { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = - std::make_shared>(limited_velocity_publisher_); + std::make_shared>( + limited_velocity_publisher_); } - const Twist empty_twist; - received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) - { stored_value = std::make_shared(empty_twist); }); + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) + { stored_value = std::make_shared(empty_twist); }); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); // initialize command subscriber - velocity_command_subscriber_ = get_node()->create_subscription( + velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -350,7 +351,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) { stored_value = std::move(msg); }); }); @@ -494,7 +495,7 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; + std::queue empty; std::swap(previous_commands_, empty); registered_left_wheel_handles_.clear(); @@ -503,7 +504,7 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) + received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) { stored_value = nullptr; }); is_halted = false; return true; From 96462c1fb6af870b9375c2ae9d606615594bbb4c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Nov 2024 22:54:27 +0000 Subject: [PATCH 3/6] Fix tricycle_controller --- tricycle_controller/src/tricycle_controller.cpp | 14 ++++++++------ .../test/test_tricycle_controller.cpp | 3 ++- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index ec7ca7bd5e..77c11de28a 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -96,7 +96,8 @@ controller_interface::return_type TricycleController::update( return controller_interface::return_type::OK; } std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) + { last_command_msg = msg; }); if (last_command_msg == nullptr) { RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); @@ -272,8 +273,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) + { stored_value = std::make_shared(empty_twist); }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive); @@ -307,7 +308,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -397,7 +399,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } @@ -433,7 +434,8 @@ bool TricycleController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); + received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) + { stored_value = nullptr; }); is_halted = false; return true; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..5e868ebeea 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } From 2edbe33bb0269cb22ee8d5122ab5d6deca0804c4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 2 Dec 2024 10:48:45 +0000 Subject: [PATCH 4/6] less code is better MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- diff_drive_controller/src/diff_drive_controller.cpp | 3 +-- tricycle_controller/src/tricycle_controller.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 67eadca2a1..025f54e388 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -504,8 +504,7 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) - { stored_value = nullptr; }); + received_velocity_msg_ptr_.set(nullptr); is_halted = false; return true; } diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 77c11de28a..689dd50d77 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -434,8 +434,7 @@ bool TricycleController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set([](std::shared_ptr & stored_value) - { stored_value = nullptr; }); + received_velocity_msg_ptr_.set(nullptr); is_halted = false; return true; } From 0ad818a7192f02774dc26affc352bb276b3a6368 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Dec 2024 22:36:30 +0000 Subject: [PATCH 5/6] Store the old message in case of a locked realtime box --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 16 ++++++++-------- .../tricycle_controller/tricycle_controller.hpp | 1 + tricycle_controller/src/tricycle_controller.cpp | 17 +++++++++-------- 4 files changed, 19 insertions(+), 16 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 74526b199c..84712fe748 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 @@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; std::queue previous_commands_; // last two commands diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 025f54e388..8984582633 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,27 +111,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) - { last_command_msg = msg; }); + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); - if (last_command_msg == nullptr) + if (last_command_msg_ == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 010a890f52..1f9361dadd 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 689dd50d77..bce05048ef 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,26 +95,27 @@ controller_interface::return_type TricycleController::update( } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr & msg) - { last_command_msg = msg; }); - if (last_command_msg == nullptr) + + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); + if (last_command_msg_ == nullptr) { RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by Limiters, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s From 67e10933cf751cbf2c2fd0ee8dd738d1ad6f0cef Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 3 Dec 2024 23:18:06 +0000 Subject: [PATCH 6/6] Initialize last_command_msg_ --- diff_drive_controller/src/diff_drive_controller.cpp | 10 +++++----- tricycle_controller/src/tricycle_controller.cpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8984582633..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -326,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) - { stored_value = std::make_shared(empty_twist); }); + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(*last_command_msg_); + previous_commands_.emplace(*last_command_msg_); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index bce05048ef..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -273,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr & stored_value) - { stored_value = std::make_shared(empty_twist); }); + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive);