From 21893a294e902bc58142f3c7e7f2be9542ffefff Mon Sep 17 00:00:00 2001 From: arthurlovekin Date: Thu, 9 Jan 2025 13:55:27 -0800 Subject: [PATCH] fix speed_limiter: update_and_write_commands() should only use reference_interfaces, not last_command_msg_ --- .../diff_drive_controller.hpp | 4 +- .../src/diff_drive_controller.cpp | 56 ++++++++++--------- 2 files changed, 30 insertions(+), 30 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 8e616b3c1b..b575df2dd7 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 @@ -126,10 +126,8 @@ class DiffDriveController : public controller_interface::ChainableControllerInte rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; - std::shared_ptr last_command_msg_; - - std::queue previous_commands_; // last two commands + std::queue> previous_two_commands_; // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1973bb1129..37771c5b6a 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -68,8 +68,8 @@ DiffDriveController::DiffDriveController() : controller_interface::ChainableControllerInterface(), // dummy limiter, will be created in on_configure // could be done with shared_ptr instead -> but will break ABI - limiter_angular_(std::numeric_limits::quiet_NaN()), - limiter_linear_(std::numeric_limits::quiet_NaN()) + limiter_linear_(std::numeric_limits::quiet_NaN()), + limiter_angular_(std::numeric_limits::quiet_NaN()) { } @@ -137,26 +137,25 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub return controller_interface::return_type::OK; } - last_command_msg_ = *(received_velocity_msg_ptr_.readFromRT()); + const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - if (last_command_msg_ == nullptr) + if (command_msg_ptr == 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 - command_msg_ptr->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; + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; } - - if (!std::isnan(last_command_msg_->twist.linear.x) && !std::isnan(last_command_msg_->twist.angular.z)) + else if (!std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z)) { - reference_interfaces_[0] = last_command_msg_->twist.linear.x; - reference_interfaces_[1] = last_command_msg_->twist.angular.z; + reference_interfaces_[0] = command_msg_ptr->twist.linear.x; + reference_interfaces_[1] = command_msg_ptr->twist.angular.z; } else { @@ -184,7 +183,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg_; double linear_command = reference_interfaces_[0]; double angular_command = reference_interfaces_[1]; @@ -283,22 +281,29 @@ controller_interface::return_type DiffDriveController::update_and_write_commands } } - auto & last_command = previous_commands_.back().twist; - auto & second_to_last_command = previous_commands_.front().twist; + double & last_linear = previous_two_commands_.back()[0]; + double & second_to_last_linear = previous_two_commands_.front()[0]; + double & last_angular = previous_two_commands_.back()[1]; + double & second_to_last_angular = previous_two_commands_.front()[1]; + limiter_linear_.limit( - linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); + linear_command, last_linear, second_to_last_linear, period.seconds()); limiter_angular_.limit( - angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); - - previous_commands_.pop(); - previous_commands_.emplace(command); + angular_command, last_angular, second_to_last_angular, period.seconds()); + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_command, angular_command}}); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; limited_velocity_command.header.stamp = time; - limited_velocity_command.twist = command.twist; + limited_velocity_command.twist.linear.x = linear_command; + limited_velocity_command.twist.linear.y = 0.0; + limited_velocity_command.twist.linear.z = 0.0; + limited_velocity_command.twist.angular.x = 0.0; + limited_velocity_command.twist.angular.y = 0.0; + limited_velocity_command.twist.angular.z = angular_command; realtime_limited_velocity_publisher_->unlockAndPublish(); } @@ -440,17 +445,14 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const int nr_ref_itfs = 2; reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + previous_two_commands_.push({{0.0, 0.0}}); //needs zeros (not NaN) to catch early accelerations + previous_two_commands_.push({{0.0, 0.0}}); std::shared_ptr empty_msg_ptr = std::make_shared(); reset_controller_reference_msg(empty_msg_ptr, get_node()); received_velocity_msg_ptr_.reset(); received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); - // Fill last two commands with default constructed commands - last_command_msg_ = std::make_shared(); - previous_commands_.emplace(*last_command_msg_); - previous_commands_.emplace(*last_command_msg_); - // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -632,8 +634,8 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + std::queue> empty; + std::swap(previous_two_commands_, empty); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear();