From f9edc41a451877e292b206069aa2e8bfd25148c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 11:16:28 +0100 Subject: [PATCH] Fix RealtimeBox API changes (#1385) --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 29 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 3 +- .../tricycle_controller.hpp | 1 + .../src/tricycle_controller.cpp | 26 +++++++++-------- .../test/test_tricycle_controller.cpp | 3 +- 6 files changed, 35 insertions(+), 28 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 d21cac602d..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_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; @@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(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( @@ -350,7 +351,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 @@ -476,7 +478,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; } 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; } 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 ec7ca7bd5e..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,25 +95,27 @@ 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); - 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 @@ -271,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(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); @@ -307,7 +309,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 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } 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; }