From d110a1c47c31219b87491f64551e1521d7a644b4 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 5 Jul 2025 14:25:59 +0530 Subject: [PATCH 1/2] Add cmd_vel_controller --- cmd_vel_controller/CMakeLists.txt | 87 ++++ cmd_vel_controller/cmd_vel_controller.xml | 7 + .../cmd_vel_controller/cmd_vel_controller.hpp | 98 +++++ cmd_vel_controller/package.xml | 30 ++ cmd_vel_controller/src/cmd_vel_controller.cpp | 399 ++++++++++++++++++ .../src/cmd_vel_controller_parameters.yaml | 389 +++++++++++++++++ 6 files changed, 1010 insertions(+) create mode 100644 cmd_vel_controller/CMakeLists.txt create mode 100644 cmd_vel_controller/cmd_vel_controller.xml create mode 100644 cmd_vel_controller/include/cmd_vel_controller/cmd_vel_controller.hpp create mode 100644 cmd_vel_controller/package.xml create mode 100644 cmd_vel_controller/src/cmd_vel_controller.cpp create mode 100644 cmd_vel_controller/src/cmd_vel_controller_parameters.yaml diff --git a/cmd_vel_controller/CMakeLists.txt b/cmd_vel_controller/CMakeLists.txt new file mode 100644 index 0000000000..3936e7bab8 --- /dev/null +++ b/cmd_vel_controller/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.16) +project(cmd_vel_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# Find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + control_toolbox + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools +) +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(${PROJECT_NAME}_parameters + src/${PROJECT_NAME}_parameters.yaml + ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp +) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + ${PROJECT_NAME}_parameters + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${geometry_msgs_TARGETS} +) +pluginlib_export_plugin_description_file(controller_interface cmd_vel_controller.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}/ +) +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + # find_package(ament_cmake_gmock REQUIRED) + # find_package(controller_manager REQUIRED) + # find_package(ros2_control_test_assets REQUIRED) + + # ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp) + # target_link_libraries(test_${PROJECT_NAME} + # ${PROJECT_NAME} + # ) + + # add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + # ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp) + # find_package(ament_lint_auto REQUIRED) + # ament_lint_auto_find_test_dependencies() + # target_link_libraries(test_load_${PROJECT_NAME} + # controller_manager::controller_manager + # ros2_control_test_assets::ros2_control_test_assets + # ) +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/cmd_vel_controller/cmd_vel_controller.xml b/cmd_vel_controller/cmd_vel_controller.xml new file mode 100644 index 0000000000..a7d2f8e5b4 --- /dev/null +++ b/cmd_vel_controller/cmd_vel_controller.xml @@ -0,0 +1,7 @@ + + + + Controller to limit cmd_vel. + + + diff --git a/cmd_vel_controller/include/cmd_vel_controller/cmd_vel_controller.hpp b/cmd_vel_controller/include/cmd_vel_controller/cmd_vel_controller.hpp new file mode 100644 index 0000000000..7baba81ea2 --- /dev/null +++ b/cmd_vel_controller/include/cmd_vel_controller/cmd_vel_controller.hpp @@ -0,0 +1,98 @@ +// Copyright 2025 Aarav Gupta +// +// 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. + +#ifndef CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_ +#define CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_ + +#include +#include + +#include "control_toolbox/rate_limiter.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" + +// auto-generated by generate_parameter_library +#include "cmd_vel_controller/cmd_vel_controller_parameters.hpp" + +namespace cmd_vel_controller +{ +class CmdVelController : public controller_interface::ChainableControllerInterface +{ + using TwistStamped = geometry_msgs::msg::TwistStamped; + +public: + CmdVelController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::vector on_export_reference_interfaces() override; + + // Parameters from ROS for cmd_vel_controller + std::shared_ptr param_listener_; + Params params_; + + // Timeout to consider cmd_vel commands old + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + // Realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; + // Save the last reference in case of unable to get value from box + TwistStamped received_command_msg_; + + std::vector> velocity_limiters_; + std::queue> previous_two_commands_; + + std::shared_ptr> cmd_vel_publisher_ = nullptr; + std::shared_ptr> realtime_cmd_vel_publisher_; + TwistStamped cmd_vel_msg_; + + bool on_set_chained_mode(bool chained_mode) override; + bool reset(); + +private: + void reset_buffers(); +}; +} // namespace cmd_vel_controller + +#endif // CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_ diff --git a/cmd_vel_controller/package.xml b/cmd_vel_controller/package.xml new file mode 100644 index 0000000000..270fef25fc --- /dev/null +++ b/cmd_vel_controller/package.xml @@ -0,0 +1,30 @@ + + + + cmd_vel_controller + 0.0.0 + Controller to limit cmd_vel + Aarav Gupta + Apache License 2.0 + + ament_cmake + + ros2_control_cmake + + controller_interface + geometry_msgs + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + + ament_cmake_gmock + controller_manager + hardware_interface + + + ament_cmake + + diff --git a/cmd_vel_controller/src/cmd_vel_controller.cpp b/cmd_vel_controller/src/cmd_vel_controller.cpp new file mode 100644 index 0000000000..436fcf9429 --- /dev/null +++ b/cmd_vel_controller/src/cmd_vel_controller.cpp @@ -0,0 +1,399 @@ +// Copyright 2025 Aarav Gupta +// +// 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. + +#include "cmd_vel_controller/cmd_vel_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_IN_TOPIC = "/cmd_vel"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "/cmd_vel_out"; +} // namespace + +namespace cmd_vel_controller +{ +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using lifecycle_msgs::msg::State; + +CmdVelController::CmdVelController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn CmdVelController::on_init() +{ + 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) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CmdVelController::on_configure(const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + // Update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } + + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + + // Allocate reference interfaces if needed + const int nr_ref_itfs = 6; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + + // Initialize speed limiters + velocity_limiters_.resize(nr_ref_itfs); + velocity_limiters_[0] = control_toolbox::RateLimiter( + 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); + velocity_limiters_[1] = control_toolbox::RateLimiter( + 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); + velocity_limiters_[2] = control_toolbox::RateLimiter( + params_.linear.z.min_velocity, params_.linear.z.max_velocity, + params_.linear.z.max_acceleration_reverse, params_.linear.z.max_acceleration, + params_.linear.z.max_deceleration, params_.linear.z.max_deceleration_reverse, + params_.linear.z.min_jerk, params_.linear.z.max_jerk); + velocity_limiters_[3] = control_toolbox::RateLimiter( + params_.angular.x.min_velocity, params_.angular.x.max_velocity, + params_.angular.x.max_acceleration_reverse, params_.angular.x.max_acceleration, + params_.angular.x.max_deceleration, params_.angular.x.max_deceleration_reverse, + params_.angular.x.min_jerk, params_.angular.x.max_jerk); + velocity_limiters_[4] = control_toolbox::RateLimiter( + params_.angular.y.min_velocity, params_.angular.y.max_velocity, + params_.angular.y.max_acceleration_reverse, params_.angular.y.max_acceleration, + params_.angular.y.max_deceleration, params_.angular.y.max_deceleration_reverse, + params_.angular.y.min_jerk, params_.angular.y.max_jerk); + velocity_limiters_[5] = control_toolbox::RateLimiter( + 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); + + // Reset the controller + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize velocity command subscriber and define its callback function + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_IN_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()->now(); + } + + const auto current_time_diff = get_node()->now() - msg->header.stamp; + + if ( + cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || + current_time_diff < cmd_vel_timeout_) + { + received_velocity_msg_.set(*msg); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Ignoring the received message (timestamp %.10f) because it is older than " + "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", + rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), + cmd_vel_timeout_.seconds()); + } + }); + + // Initialize cmd_vel publisher and message + cmd_vel_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_cmd_vel_publisher_ = + std::make_shared>(cmd_vel_publisher_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +InterfaceConfiguration CmdVelController::command_interface_configuration() const +{ + std::vector conf_names; + for (const std::string & interface_name : params_.interface_names) + { + if (interface_name != "") + { + conf_names.push_back(interface_name); + } + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration CmdVelController::state_interface_configuration() const +{ + return {interface_configuration_type::NONE, {}}; +} + +controller_interface::CallbackReturn CmdVelController::on_activate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CmdVelController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + reset_buffers(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type CmdVelController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + auto logger = get_node()->get_logger(); + + auto current_ref_op = received_velocity_msg_.try_get(); + if (current_ref_op.has_value()) + { + received_command_msg_ = current_ref_op.value(); + } + cmd_vel_msg_.header.frame_id = received_command_msg_.header.frame_id; + + const auto age_of_last_command = time - received_command_msg_.header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + reference_interfaces_[3] = 0.0; + reference_interfaces_[4] = 0.0; + reference_interfaces_[5] = 0.0; + } + else if ( + std::isfinite(received_command_msg_.twist.linear.x) && + std::isfinite(received_command_msg_.twist.linear.y) && + std::isfinite(received_command_msg_.twist.linear.z) && + std::isfinite(received_command_msg_.twist.angular.x) && + std::isfinite(received_command_msg_.twist.angular.y) && + std::isfinite(received_command_msg_.twist.angular.z)) + { + reference_interfaces_[0] = received_command_msg_.twist.linear.x; + reference_interfaces_[1] = received_command_msg_.twist.linear.y; + reference_interfaces_[2] = received_command_msg_.twist.linear.z; + reference_interfaces_[3] = received_command_msg_.twist.angular.x; + reference_interfaces_[4] = received_command_msg_.twist.angular.y; + reference_interfaces_[5] = received_command_msg_.twist.angular.z; + } + else + { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + "Command message contains NaNs. Not updating reference interfaces."); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type CmdVelController::update_and_write_commands( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + rclcpp::Logger logger = get_node()->get_logger(); + + // Update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } + + for (size_t i = 0; i < 6; ++i) + { + if (!std::isfinite(reference_interfaces_[i])) + { + return controller_interface::return_type::OK; + } + } + + std::vector command(reference_interfaces_.begin(), reference_interfaces_.begin() + 6); + + // Apply rate limiting to the command and record the smallest limiting factor + double limiting_factor = 1.0; + for (size_t i = 0; i < 6; ++i) + { + double new_limiting_factor = velocity_limiters_[i].limit( + command[i], previous_two_commands_.back()[i], previous_two_commands_.front()[i], + period.seconds()); + if (new_limiting_factor < limiting_factor) + { + limiting_factor = new_limiting_factor; + } + } + // Limit all velocities with the smallest limiting factor + for (size_t i = 0; i < 6; ++i) + { + command[i] = reference_interfaces_[i] * limiting_factor; + } + previous_two_commands_.pop(); + previous_two_commands_.push(command); + + for (size_t i = 0; i < 6; ++i) + { + bool set_command_result = true; + if (params_.interface_names[i] != "") + { + // Get command handle`` + const std::string interface_name = params_.interface_names[i]; + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&interface_name](const auto & interface) + { return interface.get_name() == interface_name; }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain command handle for %s", interface_name.c_str()); + return controller_interface::return_type::ERROR; + } + // Write command + set_command_result &= command_handle->set_value(command[i]); + } + RCLCPP_DEBUG_EXPRESSION( + logger, !set_command_result, "Unable to set the command to one of the command handles!"); + } + + if (params_.publish_cmd_vel) + { + cmd_vel_msg_.header.stamp = get_node()->now(); + cmd_vel_msg_.twist.linear.x = command[0]; + cmd_vel_msg_.twist.linear.y = command[1]; + cmd_vel_msg_.twist.linear.z = command[2]; + cmd_vel_msg_.twist.angular.x = command[3]; + cmd_vel_msg_.twist.angular.y = command[4]; + cmd_vel_msg_.twist.angular.z = command[5]; + realtime_cmd_vel_publisher_->try_publish(cmd_vel_msg_); + } + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn CmdVelController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CmdVelController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +bool CmdVelController::reset() +{ + reset_buffers(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + + return true; +} + +void CmdVelController::reset_buffers() +{ + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. + std::queue> empty; + std::swap(previous_two_commands_, empty); + previous_two_commands_.push(std::vector(6, 0.0)); + previous_two_commands_.push(std::vector(6, 0.0)); + // Fill RealtimeBox with NaNs so it will contain a known value + // but still indicate that no command has yet been sent. + received_command_msg_.header.stamp = get_node()->now(); + received_command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); + received_command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); + received_command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); + received_command_msg_.twist.angular.x = std::numeric_limits::quiet_NaN(); + received_command_msg_.twist.angular.y = std::numeric_limits::quiet_NaN(); + received_command_msg_.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(received_command_msg_); +} + +bool CmdVelController::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +std::vector CmdVelController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/linear/z", + "/angular/x", "/angular/y", "/angular/z"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + reference_interface_names[i], hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[i])); + } + + return reference_interfaces; +} +} // namespace cmd_vel_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cmd_vel_controller::CmdVelController, controller_interface::ChainableControllerInterface) diff --git a/cmd_vel_controller/src/cmd_vel_controller_parameters.yaml b/cmd_vel_controller/src/cmd_vel_controller_parameters.yaml new file mode 100644 index 0000000000..c5b45896d9 --- /dev/null +++ b/cmd_vel_controller/src/cmd_vel_controller_parameters.yaml @@ -0,0 +1,389 @@ +cmd_vel_controller: + interface_names: + { + type: string_array, + default_value: ["", "", "", "", "", ""], + description: "Names of the reference interfaces of another controller that will be sent the filtered cmd_vel. Should be in the following order (leave an empty string if not present): linear/x, linear/y, linear/z, angular/x, angular/y, angular/z.", + validation: { fixed_size<>: [6] }, + read_only: true, + } + cmd_vel_timeout: + { + type: double, + default_value: 0.5, + description: "Timeout in seconds, after which input cmd_vel is considered stale.", + } + publish_cmd_vel: + { + type: bool, + default_value: true, + description: "Whether to publish the filtered cmd_vel on a topic.", + } + linear: + x: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + y: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + z: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + angular: + x: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + y: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + z: + max_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_velocity: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_acceleration: + { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + max_deceleration: + { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } + 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] }, + read_only: true, + } + 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] }, + read_only: true, + } + max_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::gt_eq_or_nan<>": [0.0] }, + read_only: true, + } + min_jerk: + { + type: double, + default_value: .NAN, + validation: { "control_filters::lt_eq_or_nan<>": [0.0] }, + read_only: true, + } From ebe4bc2aae49e527b6ddf2fcff10edd7f784dddb Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 5 Jul 2025 20:10:25 +0530 Subject: [PATCH 2/2] Update package.xml --- cmd_vel_controller/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmd_vel_controller/package.xml b/cmd_vel_controller/package.xml index 270fef25fc..432e579de3 100644 --- a/cmd_vel_controller/package.xml +++ b/cmd_vel_controller/package.xml @@ -12,9 +12,11 @@ ros2_control_cmake controller_interface + control_toolbox geometry_msgs generate_parameter_library hardware_interface + lifecycle_msgs pluginlib rclcpp rclcpp_lifecycle @@ -22,7 +24,6 @@ ament_cmake_gmock controller_manager - hardware_interface ament_cmake