From 45735245acbc2c87352e150aefdad1b0791b7f5b Mon Sep 17 00:00:00 2001 From: kmakd Date: Fri, 9 May 2025 09:51:24 +0000 Subject: [PATCH 1/6] update mecanum controller --- .clang-format | 21 + .../mecanum_drive_controller.hpp | 86 ++-- .../src/mecanum_drive_controller.cpp | 481 +++++++++--------- 3 files changed, 315 insertions(+), 273 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..e76a1de --- /dev/null +++ b/.clang-format @@ -0,0 +1,21 @@ +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeBlocks: Preserve +AlignOperands: true +PenaltyBreakAssignment: 21 +PenaltyBreakBeforeFirstCallParameter: 1 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 2be9d1c..d26561c 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 @@ -31,29 +31,30 @@ #include #include -#include "controller_interface/controller_interface.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp" #include "mecanum_drive_controller/odometry.hpp" #include "mecanum_drive_controller/speed_limiter.hpp" #include "mecanum_drive_controller/visibility_control.h" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" -#include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_publisher.hpp" -#include "tf2_msgs/msg/tf_message.hpp" - -#include namespace mecanum_drive_controller { class MecanumDriveController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; public: MECANUM_DRIVE_CONTROLLER_PUBLIC @@ -66,28 +67,35 @@ class MecanumDriveController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override; + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; protected: struct WheelHandle @@ -96,9 +104,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface std::reference_wrapper velocity; }; - const char* feedback_type() const; - controller_interface::CallbackReturn configure_wheel(const std::string& wheel_name, - std::unique_ptr& registered_handle); + const char * feedback_type() const; + controller_interface::CallbackReturn configure_wheel( + const std::string & wheel_name, std::unique_ptr & registered_handle); std::string front_left_wheel_name_; std::string front_right_wheel_name_; @@ -117,21 +125,23 @@ class MecanumDriveController : public controller_interface::ControllerInterface Odometry odometry_; std::shared_ptr> odometry_publisher_ = nullptr; - std::shared_ptr> realtime_odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; - std::shared_ptr> odometry_transform_publisher_ = nullptr; - std::shared_ptr> realtime_odometry_transform_publisher_ = - nullptr; + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{ 500 }; + std::chrono::milliseconds cmd_vel_timeout_{500}; 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::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_x_; @@ -139,20 +149,24 @@ class MecanumDriveController : public controller_interface::ControllerInterface 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 }; + rclcpp::Time previous_update_timestamp_{0}; // publish rate limiter double publish_rate_ = 50.0; rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{ 0 }; + rclcpp::Time previous_publish_timestamp_{0}; bool is_halted = false; bool reset(); void halt(); + +private: + void reset_buffers(); }; } // namespace mecanum_drive_controller #endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index e035d6d..3a90a75 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -27,9 +27,9 @@ #include #include -#include "mecanum_drive_controller/mecanum_drive_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" #include "rclcpp/logging.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -51,25 +51,20 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -MecanumDriveController::MecanumDriveController() : controller_interface::ControllerInterface() -{ -} +MecanumDriveController::MecanumDriveController() : controller_interface::ControllerInterface() {} -const char* MecanumDriveController::feedback_type() const +const char * MecanumDriveController::feedback_type() const { return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; } controller_interface::CallbackReturn MecanumDriveController::on_init() { - try - { + try { // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - } - catch (const std::exception & e) - { + } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -84,7 +79,7 @@ InterfaceConfiguration MecanumDriveController::command_interface_configuration() conf_names.push_back(front_right_wheel_name_ + "/" + HW_IF_VELOCITY); conf_names.push_back(rear_left_wheel_name_ + "/" + HW_IF_VELOCITY); conf_names.push_back(rear_right_wheel_name_ + "/" + HW_IF_VELOCITY); - return { interface_configuration_type::INDIVIDUAL, conf_names }; + return {interface_configuration_type::INDIVIDUAL, conf_names}; } InterfaceConfiguration MecanumDriveController::state_interface_configuration() const @@ -94,36 +89,31 @@ InterfaceConfiguration MecanumDriveController::state_interface_configuration() c conf_names.push_back(front_right_wheel_name_ + "/" + feedback_type()); conf_names.push_back(rear_left_wheel_name_ + "/" + feedback_type()); conf_names.push_back(rear_right_wheel_name_ + "/" + feedback_type()); - return { interface_configuration_type::INDIVIDUAL, conf_names }; + return {interface_configuration_type::INDIVIDUAL, conf_names}; } -controller_interface::return_type MecanumDriveController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) +controller_interface::return_type MecanumDriveController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { + if (!is_halted) { halt(); is_halted = true; } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + const std::shared_ptr last_command_msg = *(received_velocity_msg_ptr_.readFromRT()); - 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; // Brake if cmd_vel has timeout, override the stored command - if (age_of_last_command > cmd_vel_timeout_) - { + if (age_of_last_command > cmd_vel_timeout_) { last_command_msg->twist.linear.x = 0.0; last_command_msg->twist.linear.y = 0.0; last_command_msg->twist.angular.z = 0.0; @@ -131,46 +121,48 @@ controller_interface::return_type MecanumDriveController::update(const rclcpp::T // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; - double& linear_command_x = command.twist.linear.x; - double& linear_command_y = command.twist.linear.y; - double& angular_command = command.twist.angular.z; + TwistStamped command = *last_command_msg; + double & linear_command_x = command.twist.linear.x; + double & linear_command_y = command.twist.linear.y; + double & angular_command = command.twist.angular.z; previous_update_timestamp_ = time; // Apply (possibly new) multipliers: - const double wheel_separation_x = params_.wheel_separation_x_multiplier * params_.wheel_separation_x; - const double wheel_separation_y = params_.wheel_separation_y_multiplier * params_.wheel_separation_y; + const double wheel_separation_x = params_.wheel_separation_x_multiplier * + params_.wheel_separation_x; + const double wheel_separation_y = params_.wheel_separation_y_multiplier * + params_.wheel_separation_y; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - if (params_.open_loop) - { + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command_x, linear_command_y, angular_command, time); - } - else - { - const double front_left_feedback = registered_front_left_wheel_handle_->feedback.get().get_value(); - const double front_right_feedback = registered_front_right_wheel_handle_->feedback.get().get_value(); - const double rear_left_feedback = registered_rear_left_wheel_handle_->feedback.get().get_value(); - const double rear_right_feedback = registered_rear_right_wheel_handle_->feedback.get().get_value(); - - if (std::isnan(front_left_feedback) || std::isnan(front_right_feedback) || std::isnan(rear_left_feedback) || - std::isnan(rear_right_feedback)) - { + } else { + const double front_left_feedback = + registered_front_left_wheel_handle_->feedback.get().get_optional().value(); + const double front_right_feedback = + registered_front_right_wheel_handle_->feedback.get().get_optional().value(); + const double rear_left_feedback = + registered_rear_left_wheel_handle_->feedback.get().get_optional().value(); + const double rear_right_feedback = + registered_rear_right_wheel_handle_->feedback.get().get_optional().value(); + + if ( + std::isnan(front_left_feedback) || std::isnan(front_right_feedback) || + std::isnan(rear_left_feedback) || std::isnan(rear_right_feedback)) { RCLCPP_ERROR(logger, "One of the wheel feedbacks %s is invalid", feedback_type()); return controller_interface::return_type::ERROR; } - if (params_.position_feedback) - { - odometry_.update(front_left_feedback, front_right_feedback, rear_left_feedback, rear_right_feedback, time); - } - else - { - odometry_.updateFromVelocity(front_left_feedback * wheel_radius * period.seconds(), - front_right_feedback * wheel_radius * period.seconds(), - rear_left_feedback * wheel_radius * period.seconds(), - rear_right_feedback * wheel_radius * period.seconds(), time); + if (params_.position_feedback) { + odometry_.update( + front_left_feedback, front_right_feedback, rear_left_feedback, rear_right_feedback, time); + } else { + odometry_.updateFromVelocity( + front_left_feedback * wheel_radius * period.seconds(), + front_right_feedback * wheel_radius * period.seconds(), + rear_left_feedback * wheel_radius * period.seconds(), + rear_right_feedback * wheel_radius * period.seconds(), time); } } @@ -178,26 +170,20 @@ controller_interface::return_type MecanumDriveController::update(const rclcpp::T orientation.setRPY(0.0, 0.0, odometry_.getHeading()); bool should_publish = false; - try - { - if (previous_publish_timestamp_ + publish_period_ < time) - { + try { + if (previous_publish_timestamp_ + publish_period_ < time) { previous_publish_timestamp_ += publish_period_; should_publish = true; } - } - catch (const std::runtime_error &) - { + } catch (const std::runtime_error &) { // Handle exceptions when the time source changes and initialize publish timestamp previous_publish_timestamp_ = time; should_publish = true; } - if (should_publish) - { - if (realtime_odometry_publisher_->trylock()) - { - auto& odometry_message = realtime_odometry_publisher_->msg_; + if (should_publish) { + if (realtime_odometry_publisher_->trylock()) { + auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); @@ -211,9 +197,8 @@ controller_interface::return_type MecanumDriveController::update(const rclcpp::T realtime_odometry_publisher_->unlockAndPublish(); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) - { - auto& transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); @@ -225,19 +210,21 @@ controller_interface::return_type MecanumDriveController::update(const rclcpp::T } } - auto& last_command = previous_commands_.back().twist; - auto& second_to_last_command = previous_commands_.front().twist; - limiter_linear_x_.limit(linear_command_x, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_linear_y_.limit(linear_command_y, last_command.linear.y, second_to_last_command.linear.y, period.seconds()); - limiter_angular_.limit(angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); + auto & last_command = previous_commands_.back().twist; + auto & second_to_last_command = previous_commands_.front().twist; + limiter_linear_x_.limit( + linear_command_x, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); + limiter_linear_y_.limit( + linear_command_y, last_command.linear.y, second_to_last_command.linear.y, 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); // Publish limited velocity - if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) - { - auto& limited_velocity_command = realtime_limited_velocity_publisher_->msg_; + 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; realtime_limited_velocity_publisher_->unlockAndPublish(); @@ -245,59 +232,64 @@ controller_interface::return_type MecanumDriveController::update(const rclcpp::T // Compute wheels velocities: const double velocity_front_left = - (linear_command_x - linear_command_y - (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / - wheel_radius; + (linear_command_x - linear_command_y - + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / + wheel_radius; const double velocity_front_right = - (linear_command_x + linear_command_y + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / - wheel_radius; + (linear_command_x + linear_command_y + + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / + wheel_radius; const double velocity_rear_left = - (linear_command_x + linear_command_y - (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / - wheel_radius; + (linear_command_x + linear_command_y - + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / + wheel_radius; const double velocity_rear_right = - (linear_command_x - linear_command_y + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / - wheel_radius; + (linear_command_x - linear_command_y + + (wheel_separation_x + wheel_separation_y) / 2. * angular_command) / + wheel_radius; // Set wheels velocities: - registered_front_left_wheel_handle_->velocity.get().set_value(velocity_front_left); - registered_front_right_wheel_handle_->velocity.get().set_value(velocity_front_right); - registered_rear_left_wheel_handle_->velocity.get().set_value(velocity_rear_left); - registered_rear_right_wheel_handle_->velocity.get().set_value(velocity_rear_right); + const bool value_set_error = + registered_front_left_wheel_handle_->velocity.get().set_value(velocity_front_left) && + registered_front_right_wheel_handle_->velocity.get().set_value(velocity_front_right) && + registered_rear_left_wheel_handle_->velocity.get().set_value(velocity_rear_left) && + registered_rear_right_wheel_handle_->velocity.get().set_value(velocity_rear_right); + + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); return controller_interface::return_type::OK; } -controller_interface::CallbackReturn MecanumDriveController::on_configure(const rclcpp_lifecycle::State&) +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State &) { auto logger = get_node()->get_logger(); // update parameters if they have changed - if (param_listener_->is_old(params_)) - { + if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); RCLCPP_INFO(logger, "Parameters were updated"); } std::string tf_prefix; - if(params_.tf_frame_prefix_enable){ - if (params_.tf_frame_prefix != "") - { + if (params_.tf_frame_prefix_enable) { + if (params_.tf_frame_prefix != "") { tf_prefix = params_.tf_frame_prefix; - } - else - { + } else { tf_prefix = std::string(get_node()->get_namespace()); } // Make sure prefix does not start with '/' and always ends with '/' - if (tf_prefix.back() != '/') - { + if (tf_prefix.back() != '/') { tf_prefix = tf_prefix + "/"; } - if (tf_prefix.front() == '/') - { + if (tf_prefix.front() == '/') { tf_prefix.erase(0, 1); } } @@ -307,22 +299,24 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const rear_left_wheel_name_ = params_.rear_left_wheel_name; rear_right_wheel_name_ = params_.rear_right_wheel_name; - if (front_left_wheel_name_.empty() || front_right_wheel_name_.empty() || rear_left_wheel_name_.empty() || - rear_right_wheel_name_.empty()) - { + if ( + front_left_wheel_name_.empty() || front_right_wheel_name_.empty() || + rear_left_wheel_name_.empty() || rear_right_wheel_name_.empty()) { RCLCPP_ERROR(logger, "Wheel name parameter is empty!"); return controller_interface::CallbackReturn::ERROR; } front_left_wheel_name_ = front_left_wheel_name_; front_right_wheel_name_ = front_right_wheel_name_; - rear_left_wheel_name_ = rear_left_wheel_name_; + rear_left_wheel_name_ = rear_left_wheel_name_; rear_right_wheel_name_ = rear_right_wheel_name_; params_.odom_frame_id = tf_prefix + params_.odom_frame_id; params_.base_frame_id = tf_prefix + params_.base_frame_id; - const double wheel_separation_x = params_.wheel_separation_y_multiplier * params_.wheel_separation_x; - const double wheel_separation_y = params_.wheel_separation_x_multiplier * params_.wheel_separation_y; + const double wheel_separation_x = params_.wheel_separation_y_multiplier * + params_.wheel_separation_x; + const double wheel_separation_y = params_.wheel_separation_x_multiplier * + params_.wheel_separation_y; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation_x, wheel_separation_y, wheel_radius); @@ -332,111 +326,92 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const std::copy(pose_diagonal.begin(), pose_diagonal.end(), params_.pose_covariance_diagonal.begin()); auto twist_diagonal = params_.twist_covariance_diagonal; - std::copy(twist_diagonal.begin(), twist_diagonal.end(), params_.twist_covariance_diagonal.begin()); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), params_.twist_covariance_diagonal.begin()); params_.open_loop = params_.open_loop; params_.position_feedback = params_.position_feedback; params_.enable_odom_tf = params_.enable_odom_tf; - cmd_vel_timeout_ = - std::chrono::milliseconds{ static_cast(params_.cmd_vel_timeout * 1000.0) }; + cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; - try - { - limiter_linear_x_ = SpeedLimiter(params_.linear.x.has_velocity_limits, - params_.linear.x.has_acceleration_limits, - params_.linear.x.has_jerk_limits, - params_.linear.x.min_velocity, - params_.linear.x.max_velocity, - params_.linear.x.min_acceleration, - params_.linear.x.max_acceleration, - params_.linear.x.min_jerk, - params_.linear.x.max_jerk); - } - catch (const std::runtime_error& e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring x linear speed limiter: %s", e.what()); + try { + limiter_linear_x_ = SpeedLimiter( + params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, + params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, + params_.linear.x.max_velocity, params_.linear.x.min_acceleration, + params_.linear.x.max_acceleration, params_.linear.x.min_jerk, params_.linear.x.max_jerk); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "Error configuring x linear speed limiter: %s", e.what()); } - try - { - limiter_linear_y_ = SpeedLimiter(params_.linear.y.has_velocity_limits, - params_.linear.y.has_acceleration_limits, - params_.linear.y.has_jerk_limits, - params_.linear.y.min_velocity, - params_.linear.y.max_velocity, - params_.linear.y.min_acceleration, - params_.linear.y.max_acceleration, - params_.linear.y.min_jerk, - params_.linear.y.max_jerk); - } - catch (const std::runtime_error& e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring y linear speed limiter: %s", e.what()); + try { + limiter_linear_y_ = SpeedLimiter( + params_.linear.y.has_velocity_limits, params_.linear.y.has_acceleration_limits, + params_.linear.y.has_jerk_limits, params_.linear.y.min_velocity, + params_.linear.y.max_velocity, params_.linear.y.min_acceleration, + params_.linear.y.max_acceleration, params_.linear.y.min_jerk, params_.linear.y.max_jerk); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "Error configuring y linear speed limiter: %s", e.what()); } - try - { - limiter_angular_ = SpeedLimiter(params_.angular.z.has_velocity_limits, - params_.angular.z.has_acceleration_limits, - params_.angular.z.has_jerk_limits, - params_.angular.z.min_velocity, - params_.angular.z.max_velocity, - params_.angular.z.min_acceleration, - params_.angular.z.max_acceleration, - params_.angular.z.min_jerk, - params_.angular.z.max_jerk); - } - catch (const std::runtime_error& e) - { + try { + limiter_angular_ = SpeedLimiter( + params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, + params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, + params_.angular.z.max_velocity, params_.angular.z.min_acceleration, + params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); + } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } - if (!reset()) - { + if (!reset()) { return controller_interface::CallbackReturn::ERROR; } - if (publish_limited_velocity_) - { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + if (publish_limited_velocity_) { + 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(std::make_shared(empty_twist)); + const TwistStamped empty_twist; + received_velocity_msg_ptr_.writeFromNonRT(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) - { - RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE(get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.writeFromNonRT(std::move(msg)); + }); // initialize odometry publisher and messasge - odometry_publisher_ = - get_node()->create_publisher(DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = - std::make_shared>(odometry_publisher_); + std::make_shared>( + odometry_publisher_); - auto& odometry_message = realtime_odometry_publisher_->msg_; + auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = params_.odom_frame_id; odometry_message.child_frame_id = params_.base_frame_id; @@ -445,11 +420,11 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); // initialize odom values zeros - odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; - for (size_t index = 0; index < 6; ++index) - { + for (size_t index = 0; index < 6; ++index) { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; @@ -457,13 +432,14 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const } // initialize transform publisher and message - odometry_transform_publisher_ = - get_node()->create_publisher(DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_transform_publisher_ = - std::make_shared>(odometry_transform_publisher_); + std::make_shared>( + odometry_transform_publisher_); // keeping track of odom and base_link transforms only - auto& odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; @@ -472,24 +448,28 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MecanumDriveController::on_activate(const rclcpp_lifecycle::State&) +controller_interface::CallbackReturn MecanumDriveController::on_activate( + const rclcpp_lifecycle::State &) { - const auto front_left_result = configure_wheel(front_left_wheel_name_, registered_front_left_wheel_handle_); - const auto front_right_result = configure_wheel(front_right_wheel_name_, registered_front_right_wheel_handle_); - const auto rear_left_result = configure_wheel(rear_left_wheel_name_, registered_rear_left_wheel_handle_); - const auto rear_right_result = configure_wheel(rear_right_wheel_name_, registered_rear_right_wheel_handle_); - - if (front_left_result == controller_interface::CallbackReturn::ERROR || - front_right_result == controller_interface::CallbackReturn::ERROR || - rear_left_result == controller_interface::CallbackReturn::ERROR || - rear_right_result == controller_interface::CallbackReturn::ERROR) - { + const auto front_left_result = configure_wheel( + front_left_wheel_name_, registered_front_left_wheel_handle_); + const auto front_right_result = configure_wheel( + front_right_wheel_name_, registered_front_right_wheel_handle_); + const auto rear_left_result = configure_wheel( + rear_left_wheel_name_, registered_rear_left_wheel_handle_); + const auto rear_right_result = configure_wheel( + rear_right_wheel_name_, registered_rear_right_wheel_handle_); + + if ( + front_left_result == controller_interface::CallbackReturn::ERROR || + front_right_result == controller_interface::CallbackReturn::ERROR || + rear_left_result == controller_interface::CallbackReturn::ERROR || + rear_right_result == controller_interface::CallbackReturn::ERROR) { return controller_interface::CallbackReturn::ERROR; } if (!(registered_front_left_wheel_handle_ && registered_front_right_wheel_handle_ && - registered_rear_left_wheel_handle_ && registered_rear_right_wheel_handle_)) - { + registered_rear_left_wheel_handle_ && registered_rear_right_wheel_handle_)) { RCLCPP_ERROR(get_node()->get_logger(), "One of wheel interfaces is non existent"); return controller_interface::CallbackReturn::ERROR; } @@ -501,27 +481,28 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate(const r return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MecanumDriveController::on_deactivate(const rclcpp_lifecycle::State&) +controller_interface::CallbackReturn MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MecanumDriveController::on_cleanup(const rclcpp_lifecycle::State&) +controller_interface::CallbackReturn MecanumDriveController::on_cleanup( + const rclcpp_lifecycle::State &) { - if (!reset()) - { + if (!reset()) { return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.writeFromNonRT(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MecanumDriveController::on_error(const rclcpp_lifecycle::State&) +controller_interface::CallbackReturn MecanumDriveController::on_error( + const rclcpp_lifecycle::State &) { - if (!reset()) - { + if (!reset()) { return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -531,9 +512,7 @@ bool MecanumDriveController::reset() { odometry_.resetOdometry(); - // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + reset_buffers(); registered_front_left_wheel_handle_.reset(); registered_front_right_wheel_handle_.reset(); @@ -543,31 +522,56 @@ bool MecanumDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); is_halted = false; return true; } -controller_interface::CallbackReturn MecanumDriveController::on_shutdown(const rclcpp_lifecycle::State&) +void MecanumDriveController::reset_buffers() +{ + // Empty out the old queue. + std::queue empty; + std::swap(previous_commands_, empty); + + // Fill RealtimeBuffer 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); +} + +controller_interface::CallbackReturn MecanumDriveController::on_shutdown( + const rclcpp_lifecycle::State &) { return controller_interface::CallbackReturn::SUCCESS; } void MecanumDriveController::halt() { - registered_front_left_wheel_handle_->velocity.get().set_value(0.0); - registered_front_right_wheel_handle_->velocity.get().set_value(0.0); - registered_rear_left_wheel_handle_->velocity.get().set_value(0.0); - registered_rear_right_wheel_handle_->velocity.get().set_value(0.0); + const bool value_set_error = + registered_front_left_wheel_handle_->velocity.get().set_value(0.0) && + registered_front_right_wheel_handle_->velocity.get().set_value(0.0) && + registered_rear_left_wheel_handle_->velocity.get().set_value(0.0) && + registered_rear_right_wheel_handle_->velocity.get().set_value(0.0); + + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); } -controller_interface::CallbackReturn -MecanumDriveController::configure_wheel(const std::string& wheel_name, std::unique_ptr& registered_handle) +controller_interface::CallbackReturn MecanumDriveController::configure_wheel( + const std::string & wheel_name, std::unique_ptr & registered_handle) { auto logger = get_node()->get_logger(); - if (wheel_name.empty()) - { + if (wheel_name.empty()) { RCLCPP_ERROR(logger, "No wheel name specified"); return controller_interface::CallbackReturn::ERROR; } @@ -575,28 +579,30 @@ MecanumDriveController::configure_wheel(const std::string& wheel_name, std::uniq // register handles const auto interface_name = feedback_type(); const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name, &interface_name](const auto& interface) { - return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == interface_name; - }); + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); - if (state_handle == state_interfaces_.cend()) - { + if (state_handle == state_interfaces_.cend()) { RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); return controller_interface::CallbackReturn::ERROR; } - const auto command_handle = - std::find_if(command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto& interface) { - return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == HW_IF_VELOCITY; - }); + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface) { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); - if (command_handle == command_interfaces_.end()) - { + if (command_handle == command_interfaces_.end()) { RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); return controller_interface::CallbackReturn::ERROR; } - registered_handle = std::make_unique(WheelHandle{ std::ref(*state_handle), std::ref(*command_handle) }); + registered_handle = + std::make_unique(WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); return controller_interface::CallbackReturn::SUCCESS; } @@ -604,4 +610,5 @@ MecanumDriveController::configure_wheel(const std::string& wheel_name, std::uniq #include "class_loader/register_macro.hpp" -CLASS_LOADER_REGISTER_CLASS(mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerInterface) +CLASS_LOADER_REGISTER_CLASS( + mecanum_drive_controller::MecanumDriveController, controller_interface::ControllerInterface) From 5211bc40e61d5ff07b0551bbc37dcd2c3a8f1b6f Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 12 May 2025 15:29:23 +0200 Subject: [PATCH 2/6] Add similar changes asi in the official ros2 control diff_drive --- .../mecanum_drive_controller.hpp | 22 +------ .../visibility_control.h | 58 ------------------- .../src/mecanum_drive_controller.cpp | 6 -- 3 files changed, 3 insertions(+), 83 deletions(-) delete mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h 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 d26561c..3999770 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 @@ -47,7 +47,6 @@ #include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp" #include "mecanum_drive_controller/odometry.hpp" #include "mecanum_drive_controller/speed_limiter.hpp" -#include "mecanum_drive_controller/visibility_control.h" #include "odometry.hpp" namespace mecanum_drive_controller @@ -57,46 +56,32 @@ class MecanumDriveController : public controller_interface::ControllerInterface using TwistStamped = geometry_msgs::msg::TwistStamped; public: - MECANUM_DRIVE_CONTROLLER_PUBLIC MecanumDriveController(); - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: struct WheelHandle { @@ -124,6 +109,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface Odometry odometry_; + // Timeout to consider cmd_vel commands old + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); + std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -132,10 +120,6 @@ class MecanumDriveController : public controller_interface::ControllerInterface nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h deleted file mode 100644 index ebe6fab..0000000 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -// Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers) - -#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define MECANUM_DRIVE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define MECANUM_DRIVE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define MECANUM_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) -#define MECANUM_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef MECANUM_DRIVE_CONTROLLER_BUILDING_DLL -#define MECANUM_DRIVE_CONTROLLER_PUBLIC MECANUM_DRIVE_CONTROLLER_EXPORT -#else -#define MECANUM_DRIVE_CONTROLLER_PUBLIC MECANUM_DRIVE_CONTROLLER_IMPORT -#endif -#define MECANUM_DRIVE_CONTROLLER_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER_PUBLIC -#define MECANUM_DRIVE_CONTROLLER_LOCAL -#else -#define MECANUM_DRIVE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define MECANUM_DRIVE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define MECANUM_DRIVE_CONTROLLER_PUBLIC -#define MECANUM_DRIVE_CONTROLLER_LOCAL -#endif -#define MECANUM_DRIVE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3a90a75..5865e8e 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -546,12 +546,6 @@ void MecanumDriveController::reset_buffers() received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); } -controller_interface::CallbackReturn MecanumDriveController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - void MecanumDriveController::halt() { const bool value_set_error = From abf18295f873e1b039fd4fd11590d114065f1d4a Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 13 May 2025 09:35:49 +0200 Subject: [PATCH 3/6] Update parameters and add deprecated info --- .../mecanum_drive_controller.hpp | 6 +- .../speed_limiter.hpp | 97 ++++++++--- .../src/mecanum_drive_controller.cpp | 89 ++++++++-- .../mecanum_drive_controller_parameter.yaml | 159 +++++++++++++++++- 4 files changed, 299 insertions(+), 52 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 3999770..e1f3a5b 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 @@ -128,9 +128,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - SpeedLimiter limiter_linear_x_; - SpeedLimiter limiter_linear_y_; - SpeedLimiter limiter_angular_; + std::unique_ptr limiter_linear_x_; + std::unique_ptr limiter_linear_y_; + std::unique_ptr limiter_angular_; bool publish_limited_velocity_ = false; std::shared_ptr> limited_velocity_publisher_ = nullptr; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp index 1acf49b..7fd352b 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp @@ -23,7 +23,9 @@ #ifndef MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -#include +#include + +#include "control_toolbox/rate_limiter.hpp" namespace mecanum_drive_controller { @@ -37,14 +39,65 @@ class SpeedLimiter * \param [in] has_jerk_limits if true, applies jerk limits * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 - * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ - SpeedLimiter(bool has_velocity_limits = false, bool has_acceleration_limits = false, bool has_jerk_limits = false, - double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, - double max_acceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); + [[deprecated]] explicit SpeedLimiter( + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + if (!has_velocity_limits) + { + min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); + } + if (!has_acceleration_limits) + { + max_deceleration = max_acceleration = std::numeric_limits::quiet_NaN(); + } + if (!has_jerk_limits) + { + min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); + } + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_deceleration, max_acceleration, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), min_jerk, + max_jerk); + } + + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually + * <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually + * >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * + * \note + * If max_* values are NAN, the respective limit is deactivated + * If min_* values are NAN (unspecified), defaults to -max + * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used + */ + explicit SpeedLimiter( + double min_velocity, double max_velocity, double max_acceleration_reverse, + double max_acceleration, double max_deceleration, double max_deceleration_reverse, + double min_jerk, double max_jerk) + { + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, + max_deceleration_reverse, min_jerk, max_jerk); + } /** * \brief Limit the velocity and acceleration @@ -54,14 +107,17 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit(double& v, double v0, double v1, double dt); + double limit(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit(v, v0, v1, dt); + } /** * \brief Limit the velocity * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double& v); + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -70,7 +126,10 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit_acceleration(double& v, double v0, double dt); + double limit_acceleration(double & v, double v0, double dt) + { + return speed_limiter_.limit_first_derivative(v, v0, dt); + } /** * \brief Limit the jerk @@ -81,25 +140,13 @@ class SpeedLimiter * \return Limiting factor (1.0 if none) * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control */ - double limit_jerk(double& v, double v0, double v1, double dt); + double limit_jerk(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit_second_derivative(v, v0, v1, dt); + } private: - // Enable/Disable velocity/acceleration/jerk limits: - bool has_velocity_limits_; - bool has_acceleration_limits_; - bool has_jerk_limits_; - - // Velocity limits: - double min_velocity_; - double max_velocity_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; - - // Jerk limits: - double min_jerk_; - double max_jerk_; + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 5865e8e..ba3c279 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -336,34 +336,93 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.linear.x.min_velocity = params_.linear.x.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.linear.x.max_deceleration = params_.linear.x.max_acceleration = + params_.linear.x.max_deceleration_reverse = params_.linear.x.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.linear.x.min_jerk = params_.linear.x.max_jerk = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.angular.z.min_velocity = params_.angular.z.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.angular.z.max_deceleration = params_.angular.z.max_acceleration = + params_.angular.z.max_deceleration_reverse = params_.angular.z.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.angular.z.min_jerk = params_.angular.z.max_jerk = + std::numeric_limits::quiet_NaN(); + } try { - limiter_linear_x_ = SpeedLimiter( - params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, - params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, - params_.linear.x.max_velocity, params_.linear.x.min_acceleration, - params_.linear.x.max_acceleration, params_.linear.x.min_jerk, params_.linear.x.max_jerk); + limiter_linear_x_ = std::make_unique( + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); } catch (const std::runtime_error & e) { RCLCPP_ERROR( get_node()->get_logger(), "Error configuring x linear speed limiter: %s", e.what()); } try { - limiter_linear_y_ = SpeedLimiter( - params_.linear.y.has_velocity_limits, params_.linear.y.has_acceleration_limits, - params_.linear.y.has_jerk_limits, params_.linear.y.min_velocity, - params_.linear.y.max_velocity, params_.linear.y.min_acceleration, - params_.linear.y.max_acceleration, params_.linear.y.min_jerk, params_.linear.y.max_jerk); + limiter_linear_y_ = std::make_unique( + params_.linear.y.min_velocity, params_.linear.y.max_velocity, + params_.linear.y.max_acceleration_reverse, params_.linear.y.max_acceleration, + params_.linear.y.max_deceleration, params_.linear.y.max_deceleration_reverse, + params_.linear.y.min_jerk, params_.linear.y.max_jerk); } catch (const std::runtime_error & e) { RCLCPP_ERROR( get_node()->get_logger(), "Error configuring y linear speed limiter: %s", e.what()); } try { - limiter_angular_ = SpeedLimiter( - params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, - params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, - params_.angular.z.max_velocity, params_.angular.z.min_acceleration, - params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); + limiter_angular_ = std::make_unique( + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml index 3c8baeb..b4396ec 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml @@ -19,16 +19,25 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Shortest distance between the front and rear wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_separation_y: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } wheel_separation_x_multiplier: { type: double, @@ -114,112 +123,244 @@ mecanum_drive_controller: x: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } y: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } angular: z: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in positive direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in positive direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } } From c4596108826eeb55ce239151a30095ffaecf1859 Mon Sep 17 00:00:00 2001 From: "rafal.gorecki" Date: Tue, 13 May 2025 16:03:02 +0200 Subject: [PATCH 4/6] Fix build --- mecanum_drive_controller/CMakeLists.txt | 42 +++--- mecanum_drive_controller/package.xml | 16 +- .../src/mecanum_drive_controller.cpp | 6 +- .../src/speed_limiter.cpp | 141 ------------------ 4 files changed, 38 insertions(+), 167 deletions(-) delete mode 100644 mecanum_drive_controller/src/speed_limiter.cpp diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index e4cd861..b394cf4 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -13,19 +13,24 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +# find_package(ros2_control_cmake REQUIRED) +# set_compiler_options() +# export_windows_symbols() + set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs + control_toolbox + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs ) find_package(ament_cmake REQUIRED) @@ -33,24 +38,25 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +# get include dirs from control_toolbox for the custom validators +get_target_property(TB_INCLUDE_DIRS control_toolbox::rate_limiter_parameters INTERFACE_INCLUDE_DIRECTORIES) generate_parameter_library(mecanum_drive_controller_parameters src/mecanum_drive_controller_parameter.yaml + ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp ) add_library(mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp src/odometry.cpp - src/speed_limiter.cpp ) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) target_include_directories(mecanum_drive_controller PUBLIC - $ + $ $ ) -target_link_libraries(mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters) +target_link_libraries(mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters + control_toolbox::rate_limiter_parameters) ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(mecanum_drive_controller PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface mecanum_drive_plugin.xml) install( diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index cd2f12b..66c2e8f 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,24 +2,32 @@ mecanum_drive_controller - 1.0.0 + 1.0.0 Controller for a mecanum drive mobile base. + + Husarion Apache License 2.0 + https://husarion.com/ + https://github.com/husarion/panther-navigation + https://github.com/husarion/panther-navigation/issues + Jakub Delicat Maciej Stępień Bence Magyar Jordan Palacios - Husarion - ament_cmake + generate_parameter_library + + control_toolbox controller_interface geometry_msgs hardware_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle rcpputils @@ -27,8 +35,6 @@ tf2 tf2_msgs - pluginlib - ament_cmake diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index ba3c279..1cc4ba2 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -212,11 +212,11 @@ controller_interface::return_type MecanumDriveController::update( auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_x_.limit( + limiter_linear_x_->limit( linear_command_x, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_linear_y_.limit( + limiter_linear_y_->limit( linear_command_y, last_command.linear.y, second_to_last_command.linear.y, period.seconds()); - limiter_angular_.limit( + limiter_angular_->limit( angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); previous_commands_.pop(); diff --git a/mecanum_drive_controller/src/speed_limiter.cpp b/mecanum_drive_controller/src/speed_limiter.cpp deleted file mode 100644 index e31434b..0000000 --- a/mecanum_drive_controller/src/speed_limiter.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// Copyright 2022 Husarion -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Author: Enrique Fernández - */ - -// Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers) -// Author: Maciej Stępień - -#include -#include - -#include "mecanum_drive_controller/speed_limiter.hpp" - -namespace mecanum_drive_controller -{ -SpeedLimiter::SpeedLimiter(bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, - double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, - double min_jerk, double max_jerk) - : has_velocity_limits_(has_velocity_limits) - , has_acceleration_limits_(has_acceleration_limits) - , has_jerk_limits_(has_jerk_limits) - , min_velocity_(min_velocity) - , max_velocity_(max_velocity) - , min_acceleration_(min_acceleration) - , max_acceleration_(max_acceleration) - , min_jerk_(min_jerk) - , max_jerk_(max_jerk) -{ - // Check if limits are valid, max must be specified, min defaults to -max if unspecified - if (has_velocity_limits_) - { - if (std::isnan(max_velocity_)) - { - throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); - } - if (std::isnan(min_velocity_)) - { - min_velocity_ = -max_velocity_; - } - } - if (has_acceleration_limits_) - { - if (std::isnan(max_acceleration_)) - { - throw std::runtime_error("Cannot apply acceleration limits if max_acceleration is not specified"); - } - if (std::isnan(min_acceleration_)) - { - min_acceleration_ = -max_acceleration_; - } - } - if (has_jerk_limits_) - { - if (std::isnan(max_jerk_)) - { - throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); - } - if (std::isnan(min_jerk_)) - { - min_jerk_ = -max_jerk_; - } - } -} - -double SpeedLimiter::limit(double& v, double v0, double v1, double dt) -{ - const double tmp = v; - - limit_jerk(v, v0, v1, dt); - limit_acceleration(v, v0, dt); - limit_velocity(v); - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_velocity(double& v) -{ - const double tmp = v; - - if (has_velocity_limits_) - { - v = std::clamp(v, min_velocity_, max_velocity_); - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) -{ - const double tmp = v; - - if (has_acceleration_limits_) - { - const double dv_min = min_acceleration_ * dt; - const double dv_max = max_acceleration_ * dt; - - const double dv = std::clamp(v - v0, dv_min, dv_max); - - v = v0 + dv; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) -{ - const double tmp = v; - - if (has_jerk_limits_) - { - const double dv = v - v0; - const double dv0 = v0 - v1; - - const double dt2 = 2. * dt * dt; - - const double da_min = min_jerk_ * dt2; - const double da_max = max_jerk_ * dt2; - - const double da = std::clamp(dv - dv0, da_min, da_max); - - v = v0 + dv0 + da; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -} // namespace mecanum_drive_controller From 1281f94f332803a2ff92f6e3c55213efab9797dc Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Wed, 14 May 2025 12:10:59 +0200 Subject: [PATCH 5/6] Apply suggestions from code review Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- mecanum_drive_controller/CMakeLists.txt | 3 --- mecanum_drive_controller/package.xml | 8 ++++---- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 65fe9e2..2578b99 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -14,9 +14,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -# find_package(ros2_control_cmake REQUIRED) set_compiler_options() -# export_windows_symbols() - set(THIS_PACKAGE_INCLUDE_DEPENDS control_toolbox controller_interface diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index da6e35b..5f7597c 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -3,17 +3,17 @@ mecanum_drive_controller 1.0.0 - Controller for a mecanum drive mobile base. - Husarion Apache License 2.0 https://husarion.com/ - https://github.com/husarion/panther-navigation - https://github.com/husarion/panther-navigation/issues + https://github.com/husarion/husarion_controllers + https://github.com/husarion/husarion_controllers/issues Jakub Delicat + Rafał Górecki + Dawid Kmak Maciej Stępień Bence Magyar Jordan Palacios From d6412f25213163b76852233d86d15fc3c20d9ed9 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 14 May 2025 12:17:39 +0200 Subject: [PATCH 6/6] Apply Dawid suggestions --- mecanum_drive_controller/CMakeLists.txt | 5 +++-- .../src/mecanum_drive_controller_parameter.yaml | 16 ++++++++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 2578b99..649fc8c 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -11,7 +11,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wold-style-cast) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -44,7 +44,6 @@ generate_parameter_library( add_library(mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp src/odometry.cpp) -target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) target_include_directories( mecanum_drive_controller PUBLIC $ @@ -57,6 +56,8 @@ ament_target_dependencies(mecanum_drive_controller PUBLIC pluginlib_export_plugin_description_file(controller_interface mecanum_drive_plugin.xml) +install(DIRECTORY include/ DESTINATION include/mecanum_drive_controller) + install( TARGETS mecanum_drive_controller mecanum_drive_controller_parameters EXPORT export_mecanum_drive_controller diff --git a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml index b4396ec..ccd0a0d 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller_parameter.yaml @@ -2,18 +2,34 @@ mecanum_drive_controller: front_left_wheel_name: { type: string, description: "Link name of the front left side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } front_right_wheel_name: { type: string, description: "Link name of the front right side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } rear_left_wheel_name: { type: string, description: "Link name of the rear left side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } rear_right_wheel_name: { type: string, description: "Link name of the rear right side wheel", + read_only: true, + validation: { + not_empty<>: null, + } } wheel_separation_x: { type: double,