From d433d581a475c3ea671a5d2c27f7107da229ae29 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Wed, 4 Mar 2026 07:14:04 +0530 Subject: [PATCH 01/33] fix: reset header stamps to zero in reset methods to prevent timeout bypass Signed-off-by: Ishan1923 --- .../src/admittance_controller.cpp | 3 ++- .../src/diff_drive_controller.cpp | 2 +- .../src/gpio_command_controller.cpp | 3 ++- .../src/mecanum_drive_controller.cpp | 3 ++- .../test/test_mecanum_drive_controller.cpp | 2 +- .../src/omni_wheel_drive_controller.cpp | 2 +- .../src/steering_controllers_library.cpp | 24 +++++++++++++++++-- 7 files changed, 31 insertions(+), 8 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 061f64b8db..d2695e162e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -41,7 +41,8 @@ void reset_wrench_msg( geometry_msgs::msg::WrenchStamped & msg, const std::shared_ptr & node) { - msg.header.stamp = node->now(); + (void)node; + msg.header.stamp = rclcpp::Time(0); msg.wrench = geometry_msgs::msg::Wrench(); } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a3d4a3999b..52e6c800a3 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -644,7 +644,7 @@ void DiffDriveController::reset_buffers() // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - command_msg_.header.stamp = get_node()->now(); + command_msg_.header.stamp = rclcpp::Time(0); command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 8ee414d7a0..799d4aef6e 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -37,7 +37,8 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces void reset_controller_reference_msg( gpio_controllers::CmdType & msg, const std::shared_ptr & node) { - msg.header.stamp = node->now(); + (void)node; + msg.header.stamp = rclcpp::Time(0); msg.interface_groups.clear(); msg.interface_values.clear(); } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2023e3b99a..9c75a4e76d 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -36,7 +36,8 @@ using ControllerReferenceMsg = void reset_controller_reference_msg( ControllerReferenceMsg & msg, const std::shared_ptr & node) { - msg.header.stamp = node->now(); + (void)node; + msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.linear.z = std::numeric_limits::quiet_NaN(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 55b8386e5d..72c5260a8b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -440,7 +440,7 @@ TEST_F( controller_->wait_for_commands(executor); reference = controller_->input_ref_.get(); - ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec); + EXPECT_GT(reference.header.stamp.sec, old_timestamp.sec); EXPECT_FALSE(std::isnan(reference.twist.linear.x)); EXPECT_FALSE(std::isnan(reference.twist.angular.z)); EXPECT_EQ(reference.twist.linear.x, 1.5); diff --git a/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp index ae0e5785bc..38de23631f 100644 --- a/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp +++ b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp @@ -566,7 +566,7 @@ void OmniWheelDriveController::reset_buffers() // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - command_msg_.header.stamp = get_node()->now(); + command_msg_.header.stamp = rclcpp::Time(0); command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 32997db6c1..1a03ad79f6 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -14,6 +14,7 @@ #include "steering_controllers_library/steering_controllers_library.hpp" +#include #include #include #include @@ -34,7 +35,8 @@ using ControllerTwistReferenceMsg = void reset_controller_reference_msg( ControllerTwistReferenceMsg & msg, const std::shared_ptr & node) { - msg.header.stamp = node->now(); + (void)node; + msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.linear.z = std::numeric_limits::quiet_NaN(); @@ -727,10 +729,28 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value()); } } - controller_state_publisher_->try_publish(controller_state_msg_); } + // store current ref (for open loop odometry) and update odometry + if (std::isfinite(reference_interfaces_[0])) + { + last_linear_velocity_ = reference_interfaces_[0]; + } + else + { + last_linear_velocity_ = 0.0; + } + if (std::isfinite(reference_interfaces_[1])) + { + last_angular_velocity_ = reference_interfaces_[1]; + } + else + { + last_angular_velocity_ = 0.0; + } + update_odometry(period); + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); From 50b2493771ebe8dd3a170934eda4d81cb0d02a8d Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Fri, 27 Mar 2026 20:43:10 +0530 Subject: [PATCH 02/33] fix: change reset_controller_reference_msg signature to remove unused node parameter --- admittance_controller/src/admittance_controller.cpp | 8 +++----- gpio_controllers/src/gpio_command_controller.cpp | 8 +++----- .../src/mecanum_drive_controller.cpp | 10 ++++------ .../src/steering_controllers_library.cpp | 7 +++---- 4 files changed, 13 insertions(+), 20 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index d2695e162e..c52bd6dd01 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -38,10 +38,8 @@ void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & // called from RT control loop void reset_wrench_msg( - geometry_msgs::msg::WrenchStamped & msg, - const std::shared_ptr & node) + geometry_msgs::msg::WrenchStamped & msg) { - (void)node; msg.header.stamp = rclcpp::Time(0); msg.wrench = geometry_msgs::msg::Wrench(); } @@ -404,7 +402,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } } reset_controller_reference_msg(joint_command_msg_); - reset_wrench_msg(wrench_command_msg_, get_node()); + reset_wrench_msg(wrench_command_msg_); // Use current joint_state as a default reference last_reference_ = joint_state_; @@ -520,7 +518,7 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( admittance_->reset(num_joints_); reset_controller_reference_msg(joint_command_msg_); - reset_wrench_msg(wrench_command_msg_, get_node()); + reset_wrench_msg(wrench_command_msg_); return CallbackReturn::SUCCESS; } diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 799d4aef6e..ac3169251f 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -34,10 +34,8 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces } // called from RT control loop -void reset_controller_reference_msg( - gpio_controllers::CmdType & msg, const std::shared_ptr & node) +void reset_controller_reference_msg(gpio_controllers::CmdType & msg) { - (void)node; msg.header.stamp = rclcpp::Time(0); msg.interface_groups.clear(); msg.interface_values.clear(); @@ -148,7 +146,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State initialize_gpio_state_msg(); // Set default value in command - reset_controller_reference_msg(gpio_commands_, get_node()); + reset_controller_reference_msg(gpio_commands_); rt_command_.try_set(gpio_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; @@ -157,7 +155,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { // Set default value in command - reset_controller_reference_msg(gpio_commands_, get_node()); + reset_controller_reference_msg(gpio_commands_); rt_command_.try_set(gpio_commands_); return CallbackReturn::SUCCESS; } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 9c75a4e76d..10d559f779 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -33,10 +33,8 @@ using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; // called from RT control loop -void reset_controller_reference_msg( - ControllerReferenceMsg & msg, const std::shared_ptr & node) +void reset_controller_reference_msg(ControllerReferenceMsg & msg) { - (void)node; msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -131,7 +129,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); // deprecation warning if tf_frame_prefix_enable set to non-default value @@ -298,7 +296,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptr & node) + ControllerTwistReferenceMsg & msg) { - (void)node; msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -265,7 +264,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); try @@ -472,7 +471,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( { // Try to set default value in command. // If this fails, then another command will be received soon anyways. - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.try_set(current_ref_); return controller_interface::CallbackReturn::SUCCESS; From cc93ee353f3b2c6ca11b11c4a4e0b30307836bb8 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Fri, 27 Mar 2026 21:06:45 +0530 Subject: [PATCH 03/33] style: pre-commit run Signed-off-by: Ishan1923 --- admittance_controller/src/admittance_controller.cpp | 3 +-- .../src/steering_controllers_library.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c52bd6dd01..8aece3791d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -37,8 +37,7 @@ void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & } // called from RT control loop -void reset_wrench_msg( - geometry_msgs::msg::WrenchStamped & msg) +void reset_wrench_msg(geometry_msgs::msg::WrenchStamped & msg) { msg.header.stamp = rclcpp::Time(0); msg.wrench = geometry_msgs::msg::Wrench(); diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index b5c819094a..04ea7cebf3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -32,8 +32,7 @@ using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // called from RT control loop -void reset_controller_reference_msg( - ControllerTwistReferenceMsg & msg) +void reset_controller_reference_msg(ControllerTwistReferenceMsg & msg) { msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); From 42104e3316c054e7ebd76f374c4af69e2ee9f240 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Fri, 3 Apr 2026 02:07:55 +0530 Subject: [PATCH 04/33] test: halt robot to clear wheel feedback before odometry reset test Signed-off-by: Ishan1923 --- .../src/steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 04ea7cebf3..f56179378c 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -781,7 +781,7 @@ bool SteeringControllersLibrary::reset() { odometry_.set_odometry(0.0, 0.0, 0.0); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); last_linear_velocity_ = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 627e5c2e6c..275e44c136 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -426,6 +426,8 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0); ASSERT_GT(controller_->odometry_.get_x(), 0.0); + move_robot(0.0, 0.0, 0.0); + // 2. Call the odometry set service auto set_request = std::make_shared(); auto set_response = std::make_shared(); From fad4d2ec892b67bb61c4b726beed6d79b0474195 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?I=C3=B1igo=20Moreno?= Date: Mon, 20 Apr 2026 21:27:15 +0200 Subject: [PATCH 05/33] Fix segfault in jtc if joint name not in urdf (#2321) --- .../src/joint_trajectory_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c907925ac7..e138279f4f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -80,7 +80,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() for (size_t i = 0; i < params_.joints.size(); ++i) { auto urdf_joint = model.getJoint(params_.joints[i]); - max_joint_vel[i] = urdf_joint->limits->velocity; + if (urdf_joint) + { + max_joint_vel[i] = urdf_joint->limits->velocity; + } if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) { RCLCPP_DEBUG( From 7debce0274b4857f042a681a4ef6696e3ff220b1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Apr 2026 08:26:12 +0000 Subject: [PATCH 06/33] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ chained_filter_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gpio_controllers/CHANGELOG.rst | 5 +++++ gps_sensor_broadcaster/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ mecanum_drive_controller/CHANGELOG.rst | 3 +++ motion_primitives_controllers/CHANGELOG.rst | 3 +++ omni_wheel_drive_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ pose_broadcaster/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ state_interfaces_broadcaster/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 29 files changed, 103 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 94622d983b..7143a19823 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3a0ee93a9f..4052dd307d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update admittance_controller to use shared 6D robot description (`#2173 `_) +* Contributors: Naitik + 6.5.0 (2026-04-02) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 6849b0e026..aaa7b9a1c3 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/chained_filter_controller/CHANGELOG.rst b/chained_filter_controller/CHANGELOG.rst index b9eeff9023..4901567c94 100644 --- a/chained_filter_controller/CHANGELOG.rst +++ b/chained_filter_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package chained_filter_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 031fd7cc9d..16214289e8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) +* RateLimiter: Don't update parameters before input checks (`#2074 `_) +* Added test for open-loop odometry with clamped input (`#2280 `_) +* Contributors: Devdoot Chatterjee, JiaHui Huang, Junius Santoso + 6.5.0 (2026-04-02) ------------------ * Fix open-loop odometry by applying SpeedLimiter first (`#2260 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3ffe1dadb8..664cd29142 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 36c70a87fe..7f4230efd1 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3042aad961..542827103b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index c18af333c5..931b6930e9 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) +* Contributors: Junius Santoso + 6.5.0 (2026-04-02) ------------------ diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst index b5638620dd..d7ae53345a 100644 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gps_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) +* Contributors: Junius Santoso + 6.5.0 (2026-04-02) ------------------ * Remove ament linters (`#2267 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index dde4209708..5581a1c71c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * Remove ament linters (`#2267 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 0db2f98082..f064c27a4d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * [JSB] Fix joint_state message corruption issue (`#2217 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 368badbda3..0d99161ffe 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix segfault in jtc if joint name not in urdf (`#2321 `_) +* Contributors: Iñigo Moreno + 6.5.0 (2026-04-02) ------------------ * GPL custom validator: Use tl_expected from libexpected-dev (`#2212 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 432fd670ce..a72a6b4053 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * mecanum_drive_controller: Don't require std_srvs (`#2213 `_) diff --git a/motion_primitives_controllers/CHANGELOG.rst b/motion_primitives_controllers/CHANGELOG.rst index 18e7feab3a..69a97a8f41 100644 --- a/motion_primitives_controllers/CHANGELOG.rst +++ b/motion_primitives_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package motion_primitives_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/omni_wheel_drive_controller/CHANGELOG.rst b/omni_wheel_drive_controller/CHANGELOG.rst index 5dba6caa77..25893c3da9 100644 --- a/omni_wheel_drive_controller/CHANGELOG.rst +++ b/omni_wheel_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package omni_wheel_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * Remove ament linters (`#2267 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 3da38f438e..f71e59d8e4 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix(parallel_gripper): rename variables for consistency (`#2314 `_) +* Contributors: Akshay Arjun + 6.5.0 (2026-04-02) ------------------ diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index f425da7649..4a7a5667ec 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 7fffcff7d7..37d282b3b2 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 6822cfd137..911aa7d89d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index a69428cd75..a138808306 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5fce6f740c..968f5a163f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 41b8e6350d..ab6d0fc8fb 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index b6d5fa5e28..ce2ae589fd 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RQT_JTC] add unit tests for parse_joint_limits (`#2281 `_) +* Contributors: Sahil Lakhmani + 6.5.0 (2026-04-02) ------------------ * rqt_jtc: Check for interface type when adding joint names (`#2231 `_) diff --git a/state_interfaces_broadcaster/CHANGELOG.rst b/state_interfaces_broadcaster/CHANGELOG.rst index 67af9a34e8..283e806e3c 100644 --- a/state_interfaces_broadcaster/CHANGELOG.rst +++ b/state_interfaces_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package state_interfaces_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index da6c97dc63..f97d4f4a09 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * set odometry service addition - steering controllers library (`#2244 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 9f62c59eff..3114724faf 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ * reset odometry service update - tricycle controller (`#2081 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4813473a27..da3acbdce4 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 2dca74fc0e..42e8d56b11 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.5.0 (2026-04-02) ------------------ From 36e027601bd5f3b284de1bd455d38ea310eca5ec Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 22 Apr 2026 08:26:29 +0000 Subject: [PATCH 07/33] 6.6.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- chained_filter_controller/CHANGELOG.rst | 4 ++-- chained_filter_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gps_sensor_broadcaster/CHANGELOG.rst | 4 ++-- gps_sensor_broadcaster/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- motion_primitives_controllers/CHANGELOG.rst | 4 ++-- motion_primitives_controllers/package.xml | 2 +- omni_wheel_drive_controller/CHANGELOG.rst | 4 ++-- omni_wheel_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- state_interfaces_broadcaster/CHANGELOG.rst | 4 ++-- state_interfaces_broadcaster/package.xml | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 60 files changed, 89 insertions(+), 89 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7143a19823..55cb6a40ec 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 4842b9289e..e200dbdc6d 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 6.5.0 + 6.6.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 4052dd307d..2936268c72 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * Update admittance_controller to use shared 6D robot description (`#2173 `_) * Contributors: Naitik diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 83dcc09ad0..95e32b817e 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 6.5.0 + 6.6.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index aaa7b9a1c3..e5c939407f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 1c210978c0..27bdb7051e 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 6.5.0 + 6.6.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/chained_filter_controller/CHANGELOG.rst b/chained_filter_controller/CHANGELOG.rst index 4901567c94..65ec96cb03 100644 --- a/chained_filter_controller/CHANGELOG.rst +++ b/chained_filter_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package chained_filter_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/chained_filter_controller/package.xml b/chained_filter_controller/package.xml index 531bf0d233..ba2f9d4c8d 100644 --- a/chained_filter_controller/package.xml +++ b/chained_filter_controller/package.xml @@ -2,7 +2,7 @@ chained_filter_controller - 6.5.0 + 6.6.0 ros2_controller for configuring filter chains Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 16214289e8..d65c4b5d21 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) * RateLimiter: Don't update parameters before input checks (`#2074 `_) * Added test for open-loop odometry with clamped input (`#2280 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d3afc2052f..93ec3398b9 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 6.5.0 + 6.6.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 664cd29142..7ae1da1087 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ba95432cd0..d4bd7761db 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 6.5.0 + 6.6.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7f4230efd1..760e04d68a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8ad6c9838b..3c6e094987 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 6.5.0 + 6.6.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 542827103b..5507ad829d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c5536de7f1..eae9e01517 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 6.5.0 + 6.6.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 931b6930e9..d4b0ca05e4 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) * Contributors: Junius Santoso diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index b6b0d03da4..443c0db0c0 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 6.5.0 + 6.6.0 Controllers to interact with gpios. Bence Magyar diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst index d7ae53345a..685b4dff11 100644 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gps_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) * Contributors: Junius Santoso diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml index 569961d798..9204eb10ab 100644 --- a/gps_sensor_broadcaster/package.xml +++ b/gps_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ gps_sensor_broadcaster - 6.5.0 + 6.6.0 Controller to publish readings of GPS sensors. Apache License 2.0 diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5581a1c71c..5ffebbd7c6 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 501dab7ec4..89d75464fc 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 6.5.0 + 6.6.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f064c27a4d..11dde2519d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 700f93fc24..706a899383 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 6.5.0 + 6.6.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0d99161ffe..c692389f90 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * Fix segfault in jtc if joint name not in urdf (`#2321 `_) * Contributors: Iñigo Moreno diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index e646573914..f9540a4da6 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 6.5.0 + 6.6.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index a72a6b4053..ae43e15c3b 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index cd76c305d0..f9493add71 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 6.5.0 + 6.6.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/motion_primitives_controllers/CHANGELOG.rst b/motion_primitives_controllers/CHANGELOG.rst index 69a97a8f41..fc4782b0bd 100644 --- a/motion_primitives_controllers/CHANGELOG.rst +++ b/motion_primitives_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package motion_primitives_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/motion_primitives_controllers/package.xml b/motion_primitives_controllers/package.xml index 0cc4ece628..d1d56f3669 100644 --- a/motion_primitives_controllers/package.xml +++ b/motion_primitives_controllers/package.xml @@ -2,7 +2,7 @@ motion_primitives_controllers - 6.5.0 + 6.6.0 Package to control robots using motion primitives like PTP, LIN and CIRC Bence Magyar diff --git a/omni_wheel_drive_controller/CHANGELOG.rst b/omni_wheel_drive_controller/CHANGELOG.rst index 25893c3da9..7d4c33a472 100644 --- a/omni_wheel_drive_controller/CHANGELOG.rst +++ b/omni_wheel_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package omni_wheel_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/omni_wheel_drive_controller/package.xml b/omni_wheel_drive_controller/package.xml index 19950fee1c..ed416b4e31 100644 --- a/omni_wheel_drive_controller/package.xml +++ b/omni_wheel_drive_controller/package.xml @@ -2,7 +2,7 @@ omni_wheel_drive_controller - 6.5.0 + 6.6.0 A chainable controller for mobile robots with omnidirectional drive with three or more omni wheels. Apache License 2.0 diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index f71e59d8e4..252c7c6f82 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * fix(parallel_gripper): rename variables for consistency (`#2314 `_) * Contributors: Akshay Arjun diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 637a8ae053..f766fc08fe 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 6.5.0 + 6.6.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 4a7a5667ec..45022e9484 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 89cfaf4a11..52a675905b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 6.5.0 + 6.6.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 37d282b3b2..f593778287 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 717ad83375..9a3052370e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 6.5.0 + 6.6.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 911aa7d89d..db6575f21c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index bd8e2b9365..159ef6af57 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 6.5.0 + 6.6.0 Generic position controller for forwarding position commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index a138808306..1930191996 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3e3d4dfde1..284db6d277 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 6.5.0 + 6.6.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 968f5a163f..917414441c 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ae1ab92128..aa7f777a59 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 6.5.0 + 6.6.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index ab6d0fc8fb..730862a4b1 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 067ff8ec68..fe7748f27b 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 6.5.0 + 6.6.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 56d824cad1..c2fea497df 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="6.5.0", + version="6.6.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ce2ae589fd..35661b6474 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ * [RQT_JTC] add unit tests for parse_joint_limits (`#2281 `_) * Contributors: Sahil Lakhmani diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 6117d2c7a3..b887eb086b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 6.5.0 + 6.6.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index b0fd029130..3860e17c10 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="6.5.0", + version="6.6.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/state_interfaces_broadcaster/CHANGELOG.rst b/state_interfaces_broadcaster/CHANGELOG.rst index 283e806e3c..d51d6dd84c 100644 --- a/state_interfaces_broadcaster/CHANGELOG.rst +++ b/state_interfaces_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package state_interfaces_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/state_interfaces_broadcaster/package.xml b/state_interfaces_broadcaster/package.xml index 162356b906..11de13f972 100644 --- a/state_interfaces_broadcaster/package.xml +++ b/state_interfaces_broadcaster/package.xml @@ -1,7 +1,7 @@ state_interfaces_broadcaster - 6.5.0 + 6.6.0 Broadcaster to publish the requested hardware interface states Bence Magyar diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index f97d4f4a09..cb5843de4b 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index a5c1b7edc2..98a0bd48fb 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 6.5.0 + 6.6.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3114724faf..9bab8a2664 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 8e0c9a1ef3..b30e1f5d01 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 6.5.0 + 6.6.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index da3acbdce4..fbd4bf7489 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 0a01e0d72a..7e45ab1d36 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 6.5.0 + 6.6.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 42e8d56b11..c8aba5d126 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.6.0 (2026-04-22) +------------------ 6.5.0 (2026-04-02) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index a67bea4fcf..27fe5b8600 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 6.5.0 + 6.6.0 Generic controller for forwarding commands. Bence Magyar From 4aaf4e70e60677d170c25507a672fa9622d9ce2b Mon Sep 17 00:00:00 2001 From: Sahil Lakhmani <126493645+lakhmanisahil@users.noreply.github.com> Date: Wed, 22 Apr 2026 22:36:16 +0530 Subject: [PATCH 08/33] [rqt_jtc] Use urdf_parser_py (#2254) --- rqt_joint_trajectory_controller/package.xml | 1 + .../joint_limits_urdf.py | 180 +++++++++--------- .../test/test_joint_limits_urdf.py | 32 ++-- 3 files changed, 111 insertions(+), 102 deletions(-) diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index b887eb086b..bd6ee64276 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -25,6 +25,7 @@ controller_manager_msgs python_qt_binding python3-rospkg + urdfdom_py trajectory_msgs rclpy rqt_gui diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 542663f1bd..5e5ae27df3 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -18,20 +18,23 @@ # https://github.com/ros/robot_model/blob/indigo-devel/ # joint_state_publisher/joint_state_publisher/joint_state_publisher -# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got -# Exception: Required attribute not set in XML: upper -# upper is an optional attribute, so I don't understand what's going on -# See comments in https://github.com/ros/urdfdom/issues/36 - -import xml.dom.minidom +import xml.etree.ElementTree as ET from math import pi import rclpy from std_msgs.msg import String +from urdf_parser_py.urdf import Robot description = "" +# Tags defined as direct children of in the URDF specification. +# Any other tag is a vendor extension (ros2_control, gazebo, xacro +# remnants, etc.) that urdf_parser_py warns about on stderr during +# parsing. We strip non-standard tags to keep logs clean. +_URDF_STANDARD_TAGS = frozenset({"link", "joint", "transmission", "material"}) + + def callback(msg): global description description = msg.data @@ -45,6 +48,24 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) +def _strip_non_urdf_tags(urdf_string): + """ + Remove non-URDF extension tags from a robot description string. + + Robot descriptions published to /robot_description commonly carry + vendor-specific extensions like and alongside + the standard URDF elements. urdf_parser_py prints warnings to stderr + for unrecognised tags (e.g. ). We strip them to keep + production logs clean. + """ + root = ET.fromstring(urdf_string) + # Iterate over a list copy because we are mutating root during the loop + for child in list(root): + if child.tag not in _URDF_STANDARD_TAGS: + root.remove(child) + return ET.tostring(root, encoding="unicode") + + def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True): """ Parse joint position and velocity limits from a URDF XML string. @@ -78,87 +99,74 @@ def parse_joint_limits(urdf_string, joints_names, use_smallest_joint_limits=True free_joints = {} dependent_joints = {} - # minidom raises xml.parsers.expat.ExpatError for completely invalid XML. - # We let that propagate naturally — the caller (get_joint_limits) can - # decide how to handle it. For unit tests, we test our own logic only. - robot = xml.dom.minidom.parseString(urdf_string).getElementsByTagName("robot")[0] - - # Walk every direct child of the element. - # Non-joint tags like , , are naturally - # skipped because we only act when localName == "joint". - for child in robot.childNodes: - # minidom includes whitespace text nodes between elements — skip them - if child.nodeType is child.TEXT_NODE: + # Strip vendor extensions to suppress urdf_parser_py stderr warnings + # for unrecognised tags like + cleaned = _strip_non_urdf_tags(urdf_string) + robot = Robot.from_xml_string(cleaned) + + for joint in robot.joints: + if joint.type == "fixed": + # Fixed joints have no DOF, so no slider is needed in the GUI + continue + + name = joint.name + + # No element at all means urdf_parser_py sets joint.limit to None + if joint.limit is None: + if name in joints_names: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) + # Joint is not managed by this controller, so we skip silently continue - if child.localName == "joint": - jtype = child.getAttribute("type") - if jtype == "fixed": - # Fixed joints have no DOF — no slider needed in the GUI - continue - name = child.getAttribute("name") - - try: - limit = child.getElementsByTagName("limit")[0] - - # minidom returns "" for absent attributes. - # float("") raises ValueError, which we catch below. - try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - # Continuous joints have no position bounds by definition - minval = -pi - maxval = pi - else: - raise Exception( - f"Missing lower/upper position limits for the joint" - f" : {name} of type : {jtype} in the robot_description!" - ) - - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - raise Exception( - f"Missing velocity limits for the joint" - f" : {name} of type : {jtype} in the robot_description!" - ) - - except IndexError: - # No element found at all for this joint - if name in joints_names: - raise Exception( - f"Missing limits tag for the joint" f" : {name} in the robot_description!" - ) - # Joint is not managed by this controller — skip silently - continue - - # Optionally narrow the range with safety controller soft limits - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - # Mimic joints follow another joint — exclude from free_joints - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + + # urdf_parser_py defaults lower/upper to 0.0 when the attributes are + # absent in the URDF. We treat min >= max as "limits missing" and + # either fall back to -pi..pi for continuous joints or raise for + # revolute joints where valid bounds are required. + minval = joint.limit.lower + maxval = joint.limit.upper + + if minval >= maxval: + if joint.type == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint" + f" : {name} of type : {joint.type} in the robot_description!" + ) + + if joint.limit.velocity is None: + raise Exception( + f"Missing velocity limits for the joint" + f" : {name} of type : {joint.type} in the robot_description!" + ) + maxvel = joint.limit.velocity + + # Optionally narrow the range with safety_controller soft limits + if use_small and joint.safety_controller is not None: + if joint.safety_controller.soft_lower_limit is not None: + minval = max(minval, joint.safety_controller.soft_lower_limit) + if joint.safety_controller.soft_upper_limit is not None: + maxval = min(maxval, joint.safety_controller.soft_upper_limit) + + # Mimic joints follow another joint and go into dependent_joints + if use_mimic and joint.mimic is not None: + entry = {"parent": joint.mimic.joint} + if joint.mimic.multiplier is not None: + entry["factor"] = joint.mimic.multiplier + if joint.mimic.offset is not None: + entry["offset"] = joint.mimic.offset + dependent_joints[name] = entry + continue + + free_joints[name] = { + "min_position": minval, + "max_position": maxval, + "has_position_limits": joint.type != "continuous", + "max_velocity": maxvel, + } return free_joints diff --git a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py index 6306c90a33..192671fd94 100644 --- a/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/test/test_joint_limits_urdf.py @@ -108,8 +108,8 @@ def test_revolute_joint_has_position_limits_true(): # --------------------------------------------------------------------------- # Group 2: Continuous joint — like a wheel, no position bounds. -# When lower/upper are absent, minidom returns "", float("") raises -# ValueError, and our code must default to -pi / +pi so the slider +# When lower/upper are absent, urdf_parser_py defaults them to 0. +# Our code detects min >= max and defaults to -pi / +pi so the slider # has a usable range. # --------------------------------------------------------------------------- @@ -190,7 +190,6 @@ def test_multiple_joints_individual_limits_correct(): # Group 5: Safety controller soft limits. # When use_smallest_joint_limits=True, soft limits should narrow the range. # When False, only the hard limits from should be used. -# This is entirely our application logic — minidom knows nothing about it. # --------------------------------------------------------------------------- @@ -260,18 +259,18 @@ def test_driver_joint_present_when_follower_is_mimic(): # --------------------------------------------------------------------------- -# Group 7: Error cases — our application logic, not minidom's. -# minidom parses all of these successfully and returns data. -# Our code is the one that decides they are errors. +# Group 7: Error cases — validation by urdf_parser_py and our application +# logic. Some errors are caught by the library during parsing, others +# by our code after parsing succeeds. # --------------------------------------------------------------------------- def test_missing_limit_tag_for_required_joint_raises(): """Joint in joints_names with no element at all must raise. - minidom parses this fine — joint.getElementsByTagName("limit") just - returns an empty list, and [0] raises IndexError. Our except block - is what turns that into a meaningful exception message. + urdf_parser_py sets joint.limit to None when no element is + present. Our code checks for this and raises an exception for joints + that the active controller manages. """ urdf = _robot( '' @@ -302,9 +301,8 @@ def test_missing_limit_tag_for_unrequired_joint_skipped_silently(): def test_revolute_joint_missing_lower_upper_raises(): """Revolute joint with no lower/upper attributes raises. - minidom returns "" for absent attributes. float("") raises ValueError. - Our except block turns that into a meaningful message for non-continuous - joints. This is our own logic — worth testing. + urdf_parser_py defaults missing lower/upper to 0. Our code detects + the invalid range (min >= max) and raises for non-continuous joints. """ urdf = _robot( '' @@ -318,10 +316,12 @@ def test_revolute_joint_missing_lower_upper_raises(): def test_missing_velocity_raises(): - """Joint with no velocity attribute raises. + """Joint with no velocity attribute raises a parse error. - minidom returns "" for absent velocity. float("") raises ValueError. - Our except block turns that into a meaningful message. Our own logic. + urdf_parser_py performs strict XML validation during parsing and + rejects a element missing the required velocity attribute. + The error comes from the library itself, before our code inspects + the joint. """ urdf = _robot( '' @@ -330,5 +330,5 @@ def test_missing_velocity_raises(): '' "" ) - with pytest.raises(Exception, match="Missing velocity limits"): + with pytest.raises(Exception, match="Required attribute not set in XML: velocity"): parse_joint_limits(urdf, ["j"]) From 66439560845579c7337fc1d1bc26b18003c34a77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 23 Apr 2026 08:24:09 +0200 Subject: [PATCH 09/33] Remove deprecated controller specializations (#2016) --- doc/controllers_index.rst | 3 - effort_controllers/CHANGELOG.rst | 376 ----------------- effort_controllers/CMakeLists.txt | 74 ---- effort_controllers/doc/userdoc.rst | 51 --- .../effort_controllers_plugins.xml | 7 - .../joint_group_effort_controller.hpp | 45 --- effort_controllers/package.xml | 43 -- .../src/joint_group_effort_controller.cpp | 74 ---- .../test_joint_group_effort_controller.yaml | 3 - .../test_joint_group_effort_controller.cpp | 207 ---------- .../test_joint_group_effort_controller.hpp | 67 ---- ...est_load_joint_group_effort_controller.cpp | 45 --- position_controllers/CHANGELOG.rst | 377 ------------------ position_controllers/CMakeLists.txt | 73 ---- position_controllers/doc/userdoc.rst | 51 --- .../joint_group_position_controller.hpp | 42 -- position_controllers/package.xml | 43 -- .../position_controllers_plugins.xml | 7 - .../src/joint_group_position_controller.cpp | 56 --- .../test_joint_group_position_controller.yaml | 3 - .../test_joint_group_position_controller.cpp | 180 --------- .../test_joint_group_position_controller.hpp | 68 ---- ...t_load_joint_group_position_controller.cpp | 46 --- ros2_controllers/doc/index.rst | 12 - ros2_controllers/package.xml | 3 - velocity_controllers/CHANGELOG.rst | 374 ----------------- velocity_controllers/CMakeLists.txt | 74 ---- velocity_controllers/doc/userdoc.rst | 51 --- .../joint_group_velocity_controller.hpp | 45 --- velocity_controllers/package.xml | 43 -- .../src/joint_group_velocity_controller.cpp | 75 ---- .../test_joint_group_velocity_controller.yaml | 3 - .../test_joint_group_velocity_controller.cpp | 204 ---------- .../test_joint_group_velocity_controller.hpp | 68 ---- ...t_load_joint_group_velocity_controller.cpp | 46 --- .../velocity_controllers_plugins.xml | 7 - 36 files changed, 2946 deletions(-) delete mode 100644 effort_controllers/CHANGELOG.rst delete mode 100644 effort_controllers/CMakeLists.txt delete mode 100644 effort_controllers/doc/userdoc.rst delete mode 100644 effort_controllers/effort_controllers_plugins.xml delete mode 100644 effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp delete mode 100644 effort_controllers/package.xml delete mode 100644 effort_controllers/src/joint_group_effort_controller.cpp delete mode 100644 effort_controllers/test/config/test_joint_group_effort_controller.yaml delete mode 100644 effort_controllers/test/test_joint_group_effort_controller.cpp delete mode 100644 effort_controllers/test/test_joint_group_effort_controller.hpp delete mode 100644 effort_controllers/test/test_load_joint_group_effort_controller.cpp delete mode 100644 position_controllers/CHANGELOG.rst delete mode 100644 position_controllers/CMakeLists.txt delete mode 100644 position_controllers/doc/userdoc.rst delete mode 100644 position_controllers/include/position_controllers/joint_group_position_controller.hpp delete mode 100644 position_controllers/package.xml delete mode 100644 position_controllers/position_controllers_plugins.xml delete mode 100644 position_controllers/src/joint_group_position_controller.cpp delete mode 100644 position_controllers/test/config/test_joint_group_position_controller.yaml delete mode 100644 position_controllers/test/test_joint_group_position_controller.cpp delete mode 100644 position_controllers/test/test_joint_group_position_controller.hpp delete mode 100644 position_controllers/test/test_load_joint_group_position_controller.cpp delete mode 100644 velocity_controllers/CHANGELOG.rst delete mode 100644 velocity_controllers/CMakeLists.txt delete mode 100644 velocity_controllers/doc/userdoc.rst delete mode 100644 velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp delete mode 100644 velocity_controllers/package.xml delete mode 100644 velocity_controllers/src/joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/test/config/test_joint_group_velocity_controller.yaml delete mode 100644 velocity_controllers/test/test_joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/test/test_joint_group_velocity_controller.hpp delete mode 100644 velocity_controllers/test/test_load_joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/velocity_controllers_plugins.xml diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index b8e9f8a8ca..70ebc1769d 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -40,13 +40,10 @@ Controllers for Manipulators and Other Robots :titlesonly: Admittance Controller <../admittance_controller/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> - Position Controllers <../position_controllers/doc/userdoc.rst> - Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst> diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst deleted file mode 100644 index 7ae1da1087..0000000000 --- a/effort_controllers/CHANGELOG.rst +++ /dev/null @@ -1,376 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package effort_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.6.0 (2026-04-22) ------------------- - -6.5.0 (2026-04-02) ------------------- - -6.4.0 (2026-03-12) ------------------- - -6.3.0 (2026-02-03) ------------------- - -6.2.0 (2025-12-31) ------------------- - -6.1.0 (2025-12-01) ------------------- -* Deprecate forward_command_controller specializations (`#1913 `_) -* Contributors: Christoph Fröhlich - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: Andy Zelenak, Jafar Abdi - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- -* Fix test dependencies (`#213 `_) -* Contributors: Bence Magyar - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt deleted file mode 100644 index ddab54bb3f..0000000000 --- a/effort_controllers/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(effort_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(effort_controllers SHARED - src/joint_group_effort_controller.cpp -) -target_compile_features(effort_controllers PUBLIC cxx_std_17) -target_include_directories(effort_controllers PUBLIC - $ - $ -) -target_link_libraries(effort_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp -) - -pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_effort_controller - test/test_load_joint_group_effort_controller.cpp - ) - target_link_libraries(test_load_joint_group_effort_controller - effort_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_effort_controller - test/test_joint_group_effort_controller.cpp - ) - target_link_libraries(test_joint_group_effort_controller - effort_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/effort_controllers -) -install( - TARGETS effort_controllers - EXPORT export_effort_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_effort_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst deleted file mode 100644 index d6189f0483..0000000000 --- a/effort_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/effort_controllers/doc/userdoc.rst - -.. _effort_controllers_userdoc: - -effort_controllers -================== - -This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. - -The package contains the following controllers: - -effort_controllers/JointGroupEffortController -------------------------------------------------- - -.. warning:: - - ``effort_controllers/JointGroupEffortController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``effort``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "effort" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' effort commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - effort_controller: - type: effort_controllers/JointGroupEffortController - - effort_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml deleted file mode 100644 index 3f7d5853ae..0000000000 --- a/effort_controllers/effort_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint effort controller commands a group of joints through the effort interface - - - diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp deleted file mode 100644 index 2855570cfd..0000000000 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace effort_controllers -{ -/** - * \brief Forward command controller for a set of effort controlled joints (linear or angular). - * - * This class forwards the commanded efforts down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b command (std_msgs::msg::Float64MultiArray) : The effort commands to apply. - */ -class JointGroupEffortController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupEffortController(); - - controller_interface::CallbackReturn on_init() override; - - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; -}; - -} // namespace effort_controllers - -#endif // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml deleted file mode 100644 index d4bd7761db..0000000000 --- a/effort_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - effort_controllers - 6.6.0 - Generic controller for forwarding commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp deleted file mode 100644 index e1dc67b57e..0000000000 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 - -#include "controller_interface/controller_interface.hpp" -#include "effort_controllers/joint_group_effort_controller.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/parameter.hpp" - -namespace effort_controllers -{ -JointGroupEffortController::JointGroupEffortController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_EFFORT; -} - -controller_interface::CallbackReturn JointGroupEffortController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'effort_controllers/JointGroupEffortController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'effort'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupEffortController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} - -controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = ForwardCommandController::on_deactivate(previous_state); - // stop all joints - for (auto & command_interface : command_interfaces_) - { - if (!command_interface.set_value(0.0)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0"); - return controller_interface::CallbackReturn::SUCCESS; - } - } - - return ret; -} - -} // namespace effort_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - effort_controllers::JointGroupEffortController, controller_interface::ControllerInterface) diff --git a/effort_controllers/test/config/test_joint_group_effort_controller.yaml b/effort_controllers/test/config/test_joint_group_effort_controller.yaml deleted file mode 100644 index 5cc99fec83..0000000000 --- a/effort_controllers/test/config/test_joint_group_effort_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_effort_controller: - ros__parameters: - joints: ["joint1"] diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp deleted file mode 100644 index 83a576a16d..0000000000 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ /dev/null @@ -1,207 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_effort_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupEffortControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupEffortControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_effort_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_, nullptr); - command_ifs.emplace_back(joint_2_cmd_, nullptr); - command_ifs.emplace_back(joint_3_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // stop the controller - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 0.0); -} diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp deleted file mode 100644 index 74562623ed..0000000000 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "effort_controllers/joint_group_effort_controller.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_EFFORT; -// subclassing and friending so we can access member variables -class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController -{ - FRIEND_TEST(JointGroupEffortControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupEffortControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupEffortControllerTest, CommandCallbackTest); - FRIEND_TEST(JointGroupEffortControllerTest, StopJointsOnDeactivateTest); -}; - -class JointGroupEffortControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_cmd_ = - std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_cmd_ = - std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_cmd_ = - std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp deleted file mode 100644 index ed6fb53838..0000000000 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupVelocityController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_effort_controller.yaml"; - - cm.set_parameter({"test_joint_group_effort_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_effort_controller.type", "effort_controllers/JointGroupEffortController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_effort_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst deleted file mode 100644 index db6575f21c..0000000000 --- a/position_controllers/CHANGELOG.rst +++ /dev/null @@ -1,377 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package position_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.6.0 (2026-04-22) ------------------- - -6.5.0 (2026-04-02) ------------------- - -6.4.0 (2026-03-12) ------------------- - -6.3.0 (2026-02-03) ------------------- - -6.2.0 (2025-12-31) ------------------- - -6.1.0 (2025-12-01) ------------------- -* Deprecate forward_command_controller specializations (`#1913 `_) -* Contributors: Christoph Fröhlich - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use gmock instead of gtest (`#1625 `_) -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich, Yassine Cherni - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- -* Update position controller package.xml (`#1431 `_) -* Contributors: Jakub "Deli" Delicat - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: Andy Zelenak, Jafar Abdi - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt deleted file mode 100644 index 48e56d6b0d..0000000000 --- a/position_controllers/CMakeLists.txt +++ /dev/null @@ -1,73 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(position_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(position_controllers SHARED - src/joint_group_position_controller.cpp -) -target_compile_features(position_controllers PUBLIC cxx_std_17) -target_include_directories(position_controllers PUBLIC - $ - $ -) -target_link_libraries(position_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp) - -pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_position_controller - test/test_load_joint_group_position_controller.cpp - ) - target_link_libraries(test_load_joint_group_position_controller - position_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_position_controller - test/test_joint_group_position_controller.cpp - ) - target_link_libraries(test_joint_group_position_controller - position_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/position_controllers -) -install( - TARGETS position_controllers - EXPORT export_position_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_position_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst deleted file mode 100644 index 7e54747ae0..0000000000 --- a/position_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/position_controllers/doc/userdoc.rst - -.. _position_controllers_userdoc: - -position_controllers -===================== - -This is a collection of controllers that work using the "position" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the position on a certain joint to achieve a set velocity. - -The package contains the following controllers: - -position_controllers/JointGroupPositionController -------------------------------------------------- - -.. warning:: - - ``position_controllers/JointGroupPositionController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``position``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "position" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' position commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - position_controller: - type: position_controllers/JointGroupPositionController - - position_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp deleted file mode 100644 index ceaf7639ed..0000000000 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace position_controllers -{ -/** - * \brief Forward command controller for a set of position controlled joints (linear or angular). - * - * This class forwards the commanded positions down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b commands (std_msgs::msg::Float64MultiArray) : The position commands to apply. - */ -class JointGroupPositionController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupPositionController(); - - controller_interface::CallbackReturn on_init() override; -}; - -} // namespace position_controllers - -#endif // POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/package.xml b/position_controllers/package.xml deleted file mode 100644 index 159ef6af57..0000000000 --- a/position_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - position_controllers - 6.6.0 - Generic position controller for forwarding position commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/position_controllers/position_controllers_plugins.xml b/position_controllers/position_controllers_plugins.xml deleted file mode 100644 index 5f8331c324..0000000000 --- a/position_controllers/position_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint position controller commands a group of joints through the position interface - - - diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp deleted file mode 100644 index 510d8e337d..0000000000 --- a/position_controllers/src/joint_group_position_controller.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 - -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/parameter.hpp" - -namespace position_controllers -{ -JointGroupPositionController::JointGroupPositionController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_POSITION; -} - -controller_interface::CallbackReturn JointGroupPositionController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'position_controllers/JointGroupPositionController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'position'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupPositionController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} -} // namespace position_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - position_controllers::JointGroupPositionController, controller_interface::ControllerInterface) diff --git a/position_controllers/test/config/test_joint_group_position_controller.yaml b/position_controllers/test/config/test_joint_group_position_controller.yaml deleted file mode 100644 index 6e4b7bee08..0000000000 --- a/position_controllers/test/config/test_joint_group_position_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_position_controller: - ros__parameters: - joints: ["joint1"] diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp deleted file mode 100644 index d0125c7e14..0000000000 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_position_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupPositionControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupPositionControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_position_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_, nullptr); - command_ifs.emplace_back(joint_2_pos_cmd_, nullptr); - command_ifs.emplace_back(joint_3_pos_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); -} diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp deleted file mode 100644 index 404f60b0ed..0000000000 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_POSITION; - -// subclassing and friending so we can access member variables -class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController -{ - FRIEND_TEST(JointGroupPositionControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupPositionControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupPositionControllerTest, CommandCallbackTest); -}; - -class JointGroupPositionControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - void SetUpHandles(); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_pos_cmd_ = - std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_pos_cmd_ = - std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_pos_cmd_ = - std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp deleted file mode 100644 index cd3fa9b945..0000000000 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupPositionController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_position_controller.yaml"; - - cm.set_parameter({"test_joint_group_position_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_position_controller.type", - "position_controllers/JointGroupPositionController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_position_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst index cb37c86f5e..ca60386888 100644 --- a/ros2_controllers/doc/index.rst +++ b/ros2_controllers/doc/index.rst @@ -26,10 +26,6 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - effort_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ * - force_torque_sensor_broadcaster - `Documentation `__ - `API `__ @@ -58,10 +54,6 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - position_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ * - range_sensor_broadcaster - `Documentation `__ - `API `__ @@ -82,7 +74,3 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - velocity_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index aa7f777a59..5e0b0b1346 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -22,7 +22,6 @@ bicycle_steering_controller chained_filter_controller diff_drive_controller - effort_controllers force_torque_sensor_broadcaster forward_command_controller gpio_controllers @@ -36,13 +35,11 @@ parallel_gripper_controller pid_controller pose_broadcaster - position_controllers range_sensor_broadcaster state_interfaces_broadcaster steering_controllers_library tricycle_controller tricycle_steering_controller - velocity_controllers ament_cmake diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst deleted file mode 100644 index c8aba5d126..0000000000 --- a/velocity_controllers/CHANGELOG.rst +++ /dev/null @@ -1,374 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package velocity_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.6.0 (2026-04-22) ------------------- - -6.5.0 (2026-04-02) ------------------- - -6.4.0 (2026-03-12) ------------------- - -6.3.0 (2026-02-03) ------------------- - -6.2.0 (2025-12-31) ------------------- - -6.1.0 (2025-12-01) ------------------- -* Deprecate forward_command_controller specializations (`#1913 `_) -* Contributors: Christoph Fröhlich - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: AndyZe, Jafar - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt deleted file mode 100644 index 8507e81d89..0000000000 --- a/velocity_controllers/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(velocity_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(velocity_controllers SHARED - src/joint_group_velocity_controller.cpp -) -target_compile_features(velocity_controllers PUBLIC cxx_std_17) -target_include_directories(velocity_controllers PUBLIC - $ - $ -) -target_link_libraries(velocity_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp) - -pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_velocity_controller - test/test_load_joint_group_velocity_controller.cpp - ) - target_link_libraries(test_load_joint_group_velocity_controller - velocity_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_velocity_controller - test/test_joint_group_velocity_controller.cpp - ) - target_link_libraries(test_joint_group_velocity_controller - velocity_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/velocity_controllers -) -install( - TARGETS - velocity_controllers - EXPORT export_velocity_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst deleted file mode 100644 index ce98a7fc45..0000000000 --- a/velocity_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/velocity_controllers/doc/userdoc.rst - -.. _velocity_controllers_userdoc: - -velocity_controllers -==================== - -This is a collection of controllers that work using the "velocity" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the velocity on a certain joint to achieve a set position. - -The package contains the following controllers: - -velocity_controllers/JointGroupVelocityController -------------------------------------------------- - -.. warning:: - - ``velocity_controllers/JointGroupVelocityController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``velocity``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "velocity" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' velocity commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - velocity_controller: - type: velocity_controllers/JointGroupVelocityController - - velocity_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp deleted file mode 100644 index 4cb0f338ea..0000000000 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace velocity_controllers -{ -/** - * \brief Forward command controller for a set of velocity controlled joints (linear or angular). - * - * This class forwards the commanded velocities down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b command (std_msgs::msg::Float64MultiArray) : The velocity commands to apply. - */ -class JointGroupVelocityController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupVelocityController(); - - controller_interface::CallbackReturn on_init() override; - - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; -}; - -} // namespace velocity_controllers - -#endif // VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml deleted file mode 100644 index 27fe5b8600..0000000000 --- a/velocity_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - velocity_controllers - 6.6.0 - Generic controller for forwarding commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp deleted file mode 100644 index fc67ead82b..0000000000 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 - -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/parameter.hpp" -#include "velocity_controllers/joint_group_velocity_controller.hpp" - -namespace velocity_controllers -{ -JointGroupVelocityController::JointGroupVelocityController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_VELOCITY; -} - -controller_interface::CallbackReturn JointGroupVelocityController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'velocity_controllers/JointGroupVelocityController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'velocity'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupVelocityController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} - -controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = ForwardCommandController::on_deactivate(previous_state); - - // stop all joints - for (auto & command_interface : command_interfaces_) - { - if (!command_interface.set_value(0.0)) - { - RCLCPP_WARN(get_node()->get_logger(), "Unable to set the command interface to value 0.0"); - return controller_interface::CallbackReturn::SUCCESS; - } - } - - return ret; -} - -} // namespace velocity_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - velocity_controllers::JointGroupVelocityController, controller_interface::ControllerInterface) diff --git a/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml b/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml deleted file mode 100644 index 6de94c007c..0000000000 --- a/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_velocity_controller: - ros__parameters: - joints: ["joint1"] diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp deleted file mode 100644 index 718de55dee..0000000000 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_velocity_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupVelocityControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupVelocityControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_velocity_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_, nullptr); - command_ifs.emplace_back(joint_2_cmd_, nullptr); - command_ifs.emplace_back(joint_3_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // stop the controller - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 0.0); -} diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp deleted file mode 100644 index fff024bd2a..0000000000 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "velocity_controllers/joint_group_velocity_controller.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_VELOCITY; - -// subclassing and friending so we can access member variables -class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController -{ - FRIEND_TEST(JointGroupVelocityControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupVelocityControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupVelocityControllerTest, CommandCallbackTest); - FRIEND_TEST(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest); -}; - -class JointGroupVelocityControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_cmd_ = - std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_cmd_ = - std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_cmd_ = - std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp deleted file mode 100644 index 377c5dd4da..0000000000 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupVelocityController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_velocity_controller.yaml"; - - cm.set_parameter({"test_joint_group_velocity_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_velocity_controller.type", - "velocity_controllers/JointGroupVelocityController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_velocity_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/velocity_controllers/velocity_controllers_plugins.xml b/velocity_controllers/velocity_controllers_plugins.xml deleted file mode 100644 index 52d410bac8..0000000000 --- a/velocity_controllers/velocity_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint velocity controller commands a group of joints through the velocity interface - - - From e8be0122f30d47171d994da8df833850f22e3b80 Mon Sep 17 00:00:00 2001 From: ahmedbilal9 Date: Sat, 25 Apr 2026 13:01:08 +0500 Subject: [PATCH 10/33] fix JTC userdoc YAML indentation and stray quote (#2327) --- joint_trajectory_controller/doc/userdoc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index ddfe7601bc..f38c2d17bd 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -48,7 +48,7 @@ This leads to the following allowed combinations of command and state interfaces Further restrictions of state interfaces exist: * ``velocity`` state interface cannot be used if ``position`` interface is missing. -* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present." +* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present. Example controller configurations can be found :ref:`below `. @@ -80,7 +80,7 @@ A yaml file for using it could be: controller_manager: ros__parameters: joint_trajectory_controller: - type: "joint_trajectory_controller/JointTrajectoryController" + type: "joint_trajectory_controller/JointTrajectoryController" joint_trajectory_controller: ros__parameters: From fdd51aebbc905edf44ee55a5cc2b425280e5f183 Mon Sep 17 00:00:00 2001 From: kamal2730 <165475282+kamal2730@users.noreply.github.com> Date: Sun, 26 Apr 2026 00:20:59 +0530 Subject: [PATCH 11/33] diff_drive: set parameters as read_only or dynamically update them (#2293) --- .../diff_drive_controller/speed_limiter.hpp | 28 +++- .../src/diff_drive_controller.cpp | 22 +++ .../src/diff_drive_controller_parameter.yaml | 17 +++ .../test/test_diff_drive_controller.cpp | 130 ++++++++++++++++++ 4 files changed, 196 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 6f101f1609..ad93bfd038 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -55,7 +55,33 @@ class SpeedLimiter min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, max_deceleration_reverse, min_jerk, max_jerk); } - + /** + * \brief Update parameters at runtime + * \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 + */ + void set_params( + 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_.set_params( + min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, + max_deceleration_reverse, min_jerk, max_jerk); + } /** * \brief Limit the velocity, acceleration, and jerk * \param [in, out] v Velocity [m/s] diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index f201724221..dafbc8b6ab 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -147,6 +147,28 @@ controller_interface::return_type DiffDriveController::update_and_write_commands { auto logger = get_node()->get_logger(); + if (param_listener_->try_update_params(params_)) + { + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + try + { + limiter_linear_->set_params( + 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); + limiter_angular_->set_params( + 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::invalid_argument & e) + { + RCLCPP_ERROR(logger, "Failed to update speed limiter parameters: %s", e.what()); + } + } + // command may be limited further by SpeedLimit, // without affecting the stored twist command double linear_command = reference_interfaces_[0]; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index b852ebc6ca..97acd56721 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -3,6 +3,7 @@ diff_drive_controller: type: string_array, default_value: [], description: "Names of the left side wheels' joints", + read_only: true, validation: { not_empty<>: [] } @@ -11,6 +12,7 @@ diff_drive_controller: type: string_array, default_value: [], description: "Names of the right side wheels' joints", + read_only: true, validation: { not_empty<>: [] } @@ -19,6 +21,7 @@ diff_drive_controller: 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.", + read_only: true, validation: { gt<>: [0.0] } @@ -27,6 +30,7 @@ diff_drive_controller: 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 than expected.", + read_only: true, validation: { gt<>: [0.0] } @@ -35,56 +39,67 @@ diff_drive_controller: type: double, default_value: 1.0, description: "Correction factor when the actual wheel separation differs from the nominal value in the ``wheel_separation`` parameter.", + read_only: true, } left_wheel_radius_multiplier: { type: double, default_value: 1.0, description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + read_only: true, } right_wheel_radius_multiplier: { type: double, default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + read_only: true, } tf_frame_prefix_enable: { type: bool, default_value: true, description: "Deprecated: this parameter will be removed in a future release. Use 'tf_frame_prefix' instead.", + read_only: true, } tf_frame_prefix: { type: string, default_value: "", description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + read_only: true, } odom_frame_id: { type: string, default_value: "odom", description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + read_only: true, } base_frame_id: { type: string, default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", + read_only: true, } pose_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, } twist_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, } open_loop: { type: bool, default_value: false, description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + read_only: true, } position_feedback: { type: bool, default_value: true, description: "Is there position feedback from hardware.", + read_only: true, } enable_odom_tf: { type: bool, @@ -105,12 +120,14 @@ diff_drive_controller: type: int, default_value: 10, description: "Size of the rolling window for calculation of mean velocity use in odometry.", + read_only: true, } # TODO(bhavin-umatiya): Remove this parameter as it is deprecated publish_rate: { type: double, default_value: 50.0, # Hz description: "Publishing rate (Hz) of the odometry and TF messages. This parameter is deprecated and will be removed in a future release.", + read_only: true, } linear: x: diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 2b5d0d4cc9..3e7d8b356a 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -672,6 +672,136 @@ TEST_F(TestDiffDriveController, test_speed_limiter) } } +TEST_F(TestDiffDriveController, test_speed_limiter_runtime_update) +{ + // If you send a linear velocity command without acceleration limits, + // then the wheel velocity command (rotations/s) will be: + // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m). + // (The velocity command looks like a step function). + // However, if there are acceleration limits, then the actual wheel velocity command + // should always be less than the ideal velocity, and should only become + // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2). + // This test verifies that parameters can be updated at runtime. + const double max_acceleration_1 = 2.0; + const double max_acceleration_2 = 5.0; + const double max_deceleration = -4.0; + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + { + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_1)), + rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(max_deceleration)), + }), + controller_interface::return_type::OK); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(configure_succeeds(controller_)); + + assignResourcesPosFeedback(); + + auto wait_for_limiter = [&](double expected_vel) + { + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(expected_vel, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(expected_vel, right_wheel_vel_cmd_->get_optional().value(), 1e-3); + } + }; + + ASSERT_TRUE(activate_succeeds(controller_)); + + waitForSetup(executor); + + // send msg + publish(0.0, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + // wait for the speed limiter to fill the queue + wait_for_limiter(0.0); + + const double dt = 0.001; + const double wheel_radius = 0.1; + // Phase 1: accelerate with max_acceleration = 2.0 + { + const double linear = 1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + const double time_acc = linear / max_acceleration_1; + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear / wheel_radius); + } + // Stop the robot + { + const double linear = 0.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + const double time_dec = 1.0 / std::abs(max_deceleration); + for (int i = 0; i < floor(time_dec / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(0.0); + } + // Phase 2: update parameter at runtime to max_acceleration = 5.0 + { + auto result = controller_->get_node()->set_parameter( + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_2))); + ASSERT_TRUE(result.successful); + } + // Phase 3: accelerate with max_acceleration = 5.0 + { + const double linear = 1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + const double time_acc_1 = linear / max_acceleration_1; + const double time_acc_2 = linear / max_acceleration_2; + // With higher acceleration, should reach target faster + ASSERT_LT(time_acc_2, time_acc_1); + for (int i = 0; i < floor(time_acc_2 / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear / wheel_radius); + } +} + TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { ASSERT_EQ( From bdd8433ac7578e5c7feaa493226e312619b11f66 Mon Sep 17 00:00:00 2001 From: Mithun Chakladar Date: Tue, 28 Apr 2026 02:01:38 +0530 Subject: [PATCH 12/33] fix(joint_state_broadcaster): suppress confusing warning for standard interfaces (#2276) --- ...nt_state_broadcaster_parameter_context.yml | 5 ++ .../src/joint_state_broadcaster.cpp | 5 +- .../joint_state_broadcaster_parameters.yaml | 6 ++- .../test/test_joint_state_broadcaster.cpp | 46 +++++++++++++++++++ .../test/test_joint_state_broadcaster.hpp | 2 + 5 files changed, 61 insertions(+), 3 deletions(-) diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml index c96f8301ae..a631ffce90 100644 --- a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -18,6 +18,11 @@ map_interface_to_joint_state: | \t\tvelocity: \t\teffort: + If ``interfaces`` explicitly contains ``position``, ``velocity``, or ``effort``, the matching + ``map_interface_to_joint_state`` entry is ignored because the standard interface is already + requested directly. In that wrong configuration, a warning is printed only when the configured + mapping value differs from the standard interface name. + Examples: diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b24152b0d8..95437b2c8d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -124,8 +124,9 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( params_.interfaces.end()) { map_interface_to_joint_state_[interface] = interface; - RCLCPP_WARN( - get_node()->get_logger(), + // Warn if custom mapping is being ignored + RCLCPP_WARN_EXPRESSION( + get_node()->get_logger(), interface != interface_to_map, "Mapping from '%s' to interface '%s' will not be done, because '%s' is defined " "in 'interface' parameter.", interface_to_map.c_str(), interface.c_str(), interface.c_str()); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 2993dddceb..cfe4104ac8 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -28,7 +28,11 @@ joint_state_broadcaster: description: "Parameter to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``joints`` parameter. If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be - published." + published. + If this parameter contains ``position``, ``velocity``, or ``effort``, the corresponding + ``map_interface_to_joint_state.`` setting is ignored because the standard interface is + already requested directly. A warning is printed only when such an ignored mapping is set to a + different custom interface name." } map_interface_to_joint_state: position: { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 53c8c0bee0..621aed3847 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -703,6 +703,52 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); } +TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingIgnoredWhenVelocityInterfaceIsRequested) +{ + constexpr auto custom_velocity_interface = "derived_velocity"; + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {HW_IF_VELOCITY}; + SetUpStateBroadcaster( + JOINT_NAMES, IF_NAMES, + {rclcpp::Parameter( + std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, custom_velocity_interface)}); + + // configure ok; a warning is expected because the custom velocity mapping is ignored + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const auto velocity_mapping = + state_broadcaster_->map_interface_to_joint_state_.find(HW_IF_VELOCITY); + ASSERT_NE(velocity_mapping, state_broadcaster_->map_interface_to_joint_state_.end()); + EXPECT_EQ(velocity_mapping->second, HW_IF_VELOCITY); + EXPECT_EQ(state_broadcaster_->map_interface_to_joint_state_.count(custom_velocity_interface), 0u); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_TRUE(std::isnan(joint_state_msg.position[0])); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[0])); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[0])); + + // publisher initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); +} + TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) { const std::vector JOINT_NAMES = {joint_names_[0]}; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 4a4e9345e9..35980611d5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -47,6 +47,8 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMapping); + FRIEND_TEST( + JointStateBroadcasterTest, TestCustomInterfaceMappingIgnoredWhenVelocityInterfaceIsRequested); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate); FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest); FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest); From c794ba671febd1585aecdbe553930608f179d1de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 Apr 2026 08:40:44 +0200 Subject: [PATCH 13/33] Bump C++ version to C++20 (#2331) --- ackermann_steering_controller/CMakeLists.txt | 1 - admittance_controller/CMakeLists.txt | 1 - bicycle_steering_controller/CMakeLists.txt | 1 - chained_filter_controller/CMakeLists.txt | 1 - diff_drive_controller/CMakeLists.txt | 1 - force_torque_sensor_broadcaster/CMakeLists.txt | 5 ----- forward_command_controller/CMakeLists.txt | 1 - gps_sensor_broadcaster/CMakeLists.txt | 1 - imu_sensor_broadcaster/CMakeLists.txt | 1 - joint_state_broadcaster/CMakeLists.txt | 1 - joint_trajectory_controller/CMakeLists.txt | 1 - mecanum_drive_controller/CMakeLists.txt | 1 - omni_wheel_drive_controller/CMakeLists.txt | 1 - parallel_gripper_controller/CMakeLists.txt | 1 - pid_controller/CMakeLists.txt | 1 - pose_broadcaster/CMakeLists.txt | 3 --- state_interfaces_broadcaster/CMakeLists.txt | 1 - steering_controllers_library/CMakeLists.txt | 1 - tricycle_controller/CMakeLists.txt | 1 - tricycle_steering_controller/CMakeLists.txt | 1 - 20 files changed, 26 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index d9dca4c145..4efd8eec87 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -35,7 +35,6 @@ add_library( SHARED src/ackermann_steering_controller.cpp ) -target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17) target_include_directories(ackermann_steering_controller PUBLIC $ $) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 30f28d1493..1cce3033f6 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -41,7 +41,6 @@ generate_parameter_library(admittance_controller_parameters add_library(admittance_controller SHARED src/admittance_controller.cpp ) -target_compile_features(admittance_controller PUBLIC cxx_std_17) target_include_directories(admittance_controller PUBLIC $ $ diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index dec1544eed..984df37cb8 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -35,7 +35,6 @@ add_library( SHARED src/bicycle_steering_controller.cpp ) -target_compile_features(bicycle_steering_controller PUBLIC cxx_std_17) target_include_directories(bicycle_steering_controller PUBLIC "$" "$") diff --git a/chained_filter_controller/CMakeLists.txt b/chained_filter_controller/CMakeLists.txt index 81d4bb7ac0..fa1d8859db 100644 --- a/chained_filter_controller/CMakeLists.txt +++ b/chained_filter_controller/CMakeLists.txt @@ -31,7 +31,6 @@ generate_parameter_library( add_library(${PROJECT_NAME} SHARED src/chained_filter.cpp ) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) target_include_directories(${PROJECT_NAME} PUBLIC $ $ diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index c0dc1788d5..53aa663be7 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -41,7 +41,6 @@ add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp ) -target_compile_features(diff_drive_controller PUBLIC cxx_std_17) target_include_directories(diff_drive_controller PUBLIC $ $ diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 6d584704a2..9eb4ba2f57 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -37,7 +37,6 @@ generate_parameter_library(wrench_transformer_parameters add_library(force_torque_sensor_broadcaster SHARED src/force_torque_sensor_broadcaster.cpp ) -target_compile_features(force_torque_sensor_broadcaster PUBLIC cxx_std_17) target_include_directories(force_torque_sensor_broadcaster PUBLIC $ $ @@ -60,7 +59,6 @@ pluginlib_export_plugin_description_file( add_library(wrench_transformer SHARED src/wrench_transformer.cpp ) -target_compile_features(wrench_transformer PUBLIC cxx_std_17) target_include_directories(wrench_transformer PUBLIC $ $ @@ -78,7 +76,6 @@ target_link_libraries(wrench_transformer PUBLIC add_executable(wrench_transformer_node src/wrench_transformer_main.cpp ) -target_compile_features(wrench_transformer_node PUBLIC cxx_std_17) target_include_directories(wrench_transformer_node PUBLIC $ $ @@ -112,7 +109,6 @@ if(BUILD_TESTING) ) ament_add_gmock(test_wrench_transformer test/test_wrench_transformer.cpp) - target_compile_features(test_wrench_transformer PUBLIC cxx_std_17) target_include_directories(test_wrench_transformer PRIVATE include) target_compile_definitions(test_wrench_transformer PRIVATE TEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") @@ -124,7 +120,6 @@ if(BUILD_TESTING) add_library(dummy_filter SHARED test/dummy_filter.cpp ) - target_compile_features(dummy_filter PUBLIC cxx_std_17) target_include_directories(dummy_filter PUBLIC $ $ diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index d75bda1196..7f4b7c5f3b 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -36,7 +36,6 @@ add_library(forward_command_controller SHARED src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_compile_features(forward_command_controller PUBLIC cxx_std_17) target_include_directories(forward_command_controller PUBLIC $ $ diff --git a/gps_sensor_broadcaster/CMakeLists.txt b/gps_sensor_broadcaster/CMakeLists.txt index 7cc818d1fe..5b1635ef1d 100644 --- a/gps_sensor_broadcaster/CMakeLists.txt +++ b/gps_sensor_broadcaster/CMakeLists.txt @@ -34,7 +34,6 @@ add_library(gps_sensor_broadcaster SHARED src/gps_sensor_broadcaster.cpp ) -target_compile_features(gps_sensor_broadcaster PUBLIC cxx_std_17) target_include_directories(gps_sensor_broadcaster PUBLIC $ $ diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 7532b2e284..53538c2522 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -30,7 +30,6 @@ generate_parameter_library(imu_sensor_broadcaster_parameters add_library(imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_compile_features(imu_sensor_broadcaster PUBLIC cxx_std_17) target_include_directories(imu_sensor_broadcaster PUBLIC $ $ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index d7f5890659..015079b00f 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -30,7 +30,6 @@ generate_parameter_library(joint_state_broadcaster_parameters add_library(joint_state_broadcaster SHARED src/joint_state_broadcaster.cpp ) -target_compile_features(joint_state_broadcaster PUBLIC cxx_std_17) target_include_directories(joint_state_broadcaster PUBLIC $ $ diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index f370fcfb3b..740f29852b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -37,7 +37,6 @@ add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) -target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) if(WIN32) target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) endif() diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 2824878632..22593a0701 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -37,7 +37,6 @@ add_library( 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 "$" "$") diff --git a/omni_wheel_drive_controller/CMakeLists.txt b/omni_wheel_drive_controller/CMakeLists.txt index af7ef7735d..3a87e8ef9f 100644 --- a/omni_wheel_drive_controller/CMakeLists.txt +++ b/omni_wheel_drive_controller/CMakeLists.txt @@ -35,7 +35,6 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}.cpp src/odometry.cpp ) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) target_include_directories(${PROJECT_NAME} PUBLIC $ $ diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 716d2b11f5..a0fba6ffdb 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -30,7 +30,6 @@ generate_parameter_library(parallel_gripper_action_controller_parameters add_library(parallel_gripper_action_controller SHARED src/parallel_gripper_action_controller.cpp ) -target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) target_include_directories(parallel_gripper_action_controller PUBLIC $ $ diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 736b2f7edd..ef9ebb3b9c 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -42,7 +42,6 @@ generate_parameter_library(pid_controller_parameters add_library(pid_controller SHARED src/pid_controller.cpp ) -target_compile_features(pid_controller PUBLIC cxx_std_17) target_include_directories(pid_controller PUBLIC $ $ diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index fd3254b96d..3d56098fb9 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -30,9 +30,6 @@ generate_parameter_library(pose_broadcaster_parameters add_library(pose_broadcaster SHARED src/pose_broadcaster.cpp ) -target_compile_features(pose_broadcaster PUBLIC - cxx_std_17 -) target_include_directories(pose_broadcaster PUBLIC $ $ diff --git a/state_interfaces_broadcaster/CMakeLists.txt b/state_interfaces_broadcaster/CMakeLists.txt index f9a1cfe170..ee08b9893c 100644 --- a/state_interfaces_broadcaster/CMakeLists.txt +++ b/state_interfaces_broadcaster/CMakeLists.txt @@ -27,7 +27,6 @@ generate_parameter_library(state_interfaces_broadcaster_parameters add_library(state_interfaces_broadcaster SHARED src/state_interfaces_broadcaster.cpp ) -target_compile_features(state_interfaces_broadcaster PUBLIC cxx_std_17) target_include_directories(state_interfaces_broadcaster PUBLIC $ $ diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index aa06d481a0..b66f1b9b9e 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -42,7 +42,6 @@ add_library( src/steering_kinematics.cpp src/steering_odometry.cpp ) -target_compile_features(steering_controllers_library PUBLIC cxx_std_17) target_include_directories(steering_controllers_library PUBLIC "$" "$") diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 0c71042d63..10e23f43de 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -41,7 +41,6 @@ add_library(tricycle_controller SHARED src/traction_limiter.cpp src/steering_limiter.cpp ) -target_compile_features(tricycle_controller PUBLIC cxx_std_17) target_include_directories(tricycle_controller PUBLIC $ $ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 22eb46bbea..65cec48ed6 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -35,7 +35,6 @@ add_library( SHARED src/tricycle_steering_controller.cpp ) -target_compile_features(tricycle_steering_controller PUBLIC cxx_std_17) target_include_directories(tricycle_steering_controller PUBLIC "$" "$") From 8dc4de092334bc2a435939d3e0cb92bc0c5f207f Mon Sep 17 00:00:00 2001 From: Ege Kural Date: Wed, 29 Apr 2026 13:06:04 -0400 Subject: [PATCH 14/33] Remove deprecated odometry reset methods (#2252) --- .../include/diff_drive_controller/odometry.hpp | 1 - diff_drive_controller/src/odometry.cpp | 7 ------- .../include/omni_wheel_drive_controller/odometry.hpp | 1 - omni_wheel_drive_controller/src/odometry.cpp | 6 ------ .../steering_controllers_library/steering_kinematics.hpp | 5 ----- .../steering_controllers_library/steering_odometry.hpp | 2 -- steering_controllers_library/src/steering_kinematics.cpp | 8 -------- steering_controllers_library/src/steering_odometry.cpp | 2 -- 8 files changed, 32 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 1a960a5e5a..d31696c204 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -56,7 +56,6 @@ class Odometry bool update_from_vel(double left_vel, double right_vel, double dt); bool try_update_open_loop(double linear_vel, double angular_vel, double dt); void setOdometry(double x, double y, double heading); - [[deprecated("Use setOdometry(0.0, 0.0, 0.0) instead")]] void resetOdometry(); double getX() const { return x_; } double getY() const { return y_; } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 05ad1121ff..aaae804c4f 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -186,13 +186,6 @@ void Odometry::setOdometry(double x, double y, double heading) heading_ = heading; resetAccumulators(); } -void Odometry::resetOdometry() -{ - x_ = 0.0; - y_ = 0.0; - heading_ = 0.0; - resetAccumulators(); -} void Odometry::setWheelParams( double wheel_separation, double left_wheel_radius, double right_wheel_radius) diff --git a/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp index b20a10a2a3..f95ec90b4f 100644 --- a/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp +++ b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp @@ -32,7 +32,6 @@ class Odometry bool updateOpenLoop( const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, const rclcpp::Time & time); - [[deprecated("Use setOdometry(0.0, 0.0, 0.0) instead")]] void resetOdometry(); void setOdometry(const double & x, const double & y, const double & heading); double getX() const { return x_; } diff --git a/omni_wheel_drive_controller/src/odometry.cpp b/omni_wheel_drive_controller/src/odometry.cpp index 574f59ded2..e746a23810 100644 --- a/omni_wheel_drive_controller/src/odometry.cpp +++ b/omni_wheel_drive_controller/src/odometry.cpp @@ -122,12 +122,6 @@ bool Odometry::updateOpenLoop( return true; } -void Odometry::resetOdometry() -{ - x_ = 0.0; - y_ = 0.0; - heading_ = 0.0; -} void Odometry::setOdometry(const double & x, const double & y, const double & heading) { x_ = x; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp index 798aaac858..983effae85 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp @@ -213,11 +213,6 @@ class SteeringKinematics const double v_bx, const double omega_bz, const bool open_loop = true, const bool reduce_wheel_speed_until_steering_reached = false); - /** - * \brief Reset poses, heading, and accumulators - */ - [[deprecated("Use set_odometry(0.0, 0.0, 0.0) instead")]] void reset_odometry(); - /** * \brief Set poses and heading with the given values and reset accumulators * \param x x position [m] diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 9fa95dd45e..53a65150c1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -114,8 +114,6 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee double v_bx, double omega_bz, bool open_loop = true, bool reduce_wheel_speed_until_steering_reached = false); - void reset_odometry(); - private: steering_kinematics::SteeringKinematics sk_impl_; }; diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp index ede8387f52..305b0e6fcf 100644 --- a/steering_controllers_library/src/steering_kinematics.cpp +++ b/steering_controllers_library/src/steering_kinematics.cpp @@ -347,14 +347,6 @@ std::tuple, std::vector> SteeringKinematics::get_com } } -void SteeringKinematics::reset_odometry() -{ - x_ = 0.0; - y_ = 0.0; - heading_ = 0.0; - reset_accumulators(); -} - void SteeringKinematics::set_odometry(const double & x, const double & y, const double & heading) { x_ = x; diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index d92fcc3e46..024ee702e8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -108,6 +108,4 @@ std::tuple, std::vector> SteeringOdometry::get_comma v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); } -void SteeringOdometry::reset_odometry() { sk_impl_.set_odometry(0.0, 0.0, 0.0); } - } // namespace steering_odometry From 52997cb5ae359836741aea72da89c18bcab6006c Mon Sep 17 00:00:00 2001 From: Bhavin Umatiya <156327414+Bhavin-umatiya@users.noreply.github.com> Date: Fri, 1 May 2026 00:11:41 +0530 Subject: [PATCH 15/33] Remove deprecated publish_rate from diff_drive_controller (#2259) --- .../diff_drive_controller.hpp | 6 -- .../src/diff_drive_controller.cpp | 79 +++++-------------- .../src/diff_drive_controller_parameter.yaml | 8 +- doc/release_notes.rst | 1 + 4 files changed, 23 insertions(+), 71 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 46a7a2c63e..eb177ab7ae 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -148,12 +148,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte rclcpp::Time previous_update_timestamp_{0}; - // publish rate limiter - // TODO(bhavin-umatiya): Remove these two member variables - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_seconds(0.0); - rclcpp::Time previous_publish_timestamp_{0}; - rclcpp::Service::SharedPtr set_odom_service_; std::atomic set_odom_requested_{false}; realtime_tools::RealtimeThreadSafeBox diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index dafbc8b6ab..69bb2d5d89 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -271,54 +271,31 @@ controller_interface::return_type DiffDriveController::update_and_write_commands tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - // TODO(bhavin-umatiya): Remove publish rate functionality - bool should_publish = false; - if (previous_publish_timestamp_.get_clock_type() != time.get_clock_type()) + if (realtime_odometry_publisher_) { - should_publish = true; - } - else if (previous_publish_timestamp_ + publish_period_ <= time) - { - should_publish = true; + odometry_message_.header.stamp = time; + odometry_message_.pose.pose.position.x = odometry_.getX(); + odometry_message_.pose.pose.position.y = odometry_.getY(); + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear.x = odometry_.getLinear(); + odometry_message_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_message_); } - if (should_publish) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - if (previous_publish_timestamp_.get_clock_type() != time.get_clock_type()) - { - previous_publish_timestamp_ = time; - } - else - { - previous_publish_timestamp_ += publish_period_; - } - - if (realtime_odometry_publisher_) - { - odometry_message_.header.stamp = time; - odometry_message_.pose.pose.position.x = odometry_.getX(); - odometry_message_.pose.pose.position.y = odometry_.getY(); - odometry_message_.pose.pose.orientation.x = orientation.x(); - odometry_message_.pose.pose.orientation.y = orientation.y(); - odometry_message_.pose.pose.orientation.z = orientation.z(); - odometry_message_.pose.pose.orientation.w = orientation.w(); - odometry_message_.twist.twist.linear.x = odometry_.getLinear(); - odometry_message_.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->try_publish(odometry_message_); - } - - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) - { - auto & transform = odometry_transform_message_.transforms.front(); - transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); - } + auto & transform = odometry_transform_message_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); } } @@ -523,19 +500,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message_.header.frame_id = odom_frame_id; odometry_message_.child_frame_id = base_frame_id; - // limit the publication on the topics /odom and /tf - publish_rate_ = params_.publish_rate; - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - - // TODO(bhavin-umatiya): Remove this warning - if (publish_rate_ > 0.0 && !std::isnan(publish_rate_)) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[deprecated] publish_rate parameter is deprecated and will be removed in a future release. " - "The publish rate of odometry and TF messages should not be limited."); - } - // initialize odom values zeros odometry_message_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); @@ -567,7 +531,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::placeholders::_2, std::placeholders::_3)); previous_update_timestamp_ = get_node()->get_clock()->now(); - previous_publish_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 97acd56721..b0155c58f5 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -122,13 +122,7 @@ diff_drive_controller: description: "Size of the rolling window for calculation of mean velocity use in odometry.", read_only: true, } - # TODO(bhavin-umatiya): Remove this parameter as it is deprecated - publish_rate: { - type: double, - default_value: 50.0, # Hz - description: "Publishing rate (Hz) of the odometry and TF messages. This parameter is deprecated and will be removed in a future release.", - read_only: true, - } + linear: x: max_velocity: { diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 6cbbbe06c6..5c9e7645a4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -18,6 +18,7 @@ diff_drive_controller * Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#1997 `_). * Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#1997 `_). * Set odometry service added to be used at runtime. (`#2096 `_). +* Removed deprecated parameter ``publish_rate`` and associated rate-limiting logic. (`#2259 `_). mecanum_drive_controller ***************************** From 9a48b9426b9e9d11a8fd181f292cc9f7d920550c Mon Sep 17 00:00:00 2001 From: Souri Rishik Date: Fri, 1 May 2026 17:14:19 +0530 Subject: [PATCH 16/33] fix: correct ASSERT_EQ to ASSERT_NE in admittance controller load test (#2264) --- .../test/test_load_admittance_controller.cpp | 2 +- admittance_controller/test/test_params.yaml | 71 +++++++++++++++++++ 2 files changed, 72 insertions(+), 1 deletion(-) diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 5006920858..026e59b422 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -36,7 +36,7 @@ TEST(TestLoadAdmittanceController, load_controller) cm.set_parameter( {"load_admittance_controller.type", "admittance_controller/AdmittanceController"}); - ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr); + ASSERT_NE(cm.load_controller("load_admittance_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 22cbda2df9..0aa568dec4 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -16,6 +16,77 @@ load_admittance_controller: - position - velocity + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: tool0 + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 test_admittance_controller: # contains minimal needed parameters for kuka_kr6 From 2b34b6764edfd78471a27749ba54f8bace18e8fa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 1 May 2026 18:07:55 +0200 Subject: [PATCH 17/33] Bump version of pre-commit hooks (#2337) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c46cfb775c..9ee9a07bd1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -64,7 +64,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v22.1.2 + rev: v22.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] From 83b2641f3105dc9fa95dbe58f7f95d67feda390c Mon Sep 17 00:00:00 2001 From: Maksim Sviridov <114253310+sviridovm@users.noreply.github.com> Date: Sun, 3 May 2026 15:47:58 -0400 Subject: [PATCH 18/33] added warning messages when unable to remap the interface properly in Joint State Broadcaster. (#2082) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Sai Kishor Kothakota Co-authored-by: Christoph Fröhlich Co-authored-by: Surya! --- .../src/joint_state_broadcaster.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 95437b2c8d..d93ee6d67e 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -260,11 +260,20 @@ bool JointStateBroadcaster::init_joint_data() name_if_value_mapping_[prefix_name] = {}; } // add interface name + std::string interface_name = si->get_interface_name(); if (map_interface_to_joint_state_.count(interface_name) > 0) { interface_name = map_interface_to_joint_state_[interface_name]; } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Interface '%s' of joint '%s' is not mapped to any joint state field. The default value %f " + "will be used.", + interface_name.c_str(), prefix_name.c_str(), kUninitializedValue); + } name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue; // filter state interfaces that have at least one of the joint_states fields, @@ -283,7 +292,18 @@ bool JointStateBroadcaster::init_joint_data() } } } + else + { + // If default interfaces (pos/vel/eff) are missing, log a warning and return NaN in + // the fields. + RCLCPP_WARN( + get_node()->get_logger(), + "Interface '%s' of joint '%s' is not present in JointState message fields. NaN's will be " + "filled in the respective field.", + interface_name.c_str(), prefix_name.c_str()); + } } + std::reverse(joint_names_.begin(), joint_names_.end()); if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty()) { From cd4e227276cbe7df1fc5a2b4bd86b6170adcda2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 7 May 2026 10:14:19 +0200 Subject: [PATCH 19/33] Suppress cppcheck errors from macros from version.h (#2346) --- admittance_controller/test/test_admittance_controller.hpp | 2 ++ .../test/test_wrench_transformer.cpp | 2 ++ .../include/joint_state_broadcaster/joint_state_broadcaster.hpp | 1 + joint_trajectory_controller/src/joint_trajectory_controller.cpp | 1 + 4 files changed, 6 insertions(+) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 5d7634fd50..c46e0b782a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -36,6 +36,7 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/version.h" +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(18, 0, 0) #include "rclcpp/node_interfaces/node_interfaces.hpp" #endif @@ -229,6 +230,7 @@ class AdmittanceControllerTest : public ::testing::Test void broadcast_tfs() { +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(18, 0, 0) static tf2_ros::TransformBroadcaster br( rclcpp::node_interfaces::NodeInterfaces( diff --git a/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp b/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp index 5f09fa1a01..6f56761603 100644 --- a/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp +++ b/force_torque_sensor_broadcaster/test/test_wrench_transformer.cpp @@ -32,6 +32,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/version.h" +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(18, 0, 0) #include "rclcpp/node_interfaces/node_interfaces.hpp" #endif @@ -60,6 +61,7 @@ class TestWrenchTransformer : public ::testing::Test { auto tf_node = std::make_shared("static_tf_broadcaster"); executor_->add_node(tf_node); +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(18, 0, 0) tf2_ros::StaticTransformBroadcaster tf_broadcaster( rclcpp::node_interfaces::NodeInterfaces( diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 05d2334d0b..7e344bf272 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -26,6 +26,7 @@ #include "sensor_msgs/msg/joint_state.hpp" #include "rclcpp/version.h" +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(29, 0, 0) #include "urdf/model.hpp" #else diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e138279f4f..c2474bbfdb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -35,6 +35,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp/version.h" +// cppcheck-suppress syntaxError #if RCLCPP_VERSION_GTE(29, 0, 0) #include "urdf/model.hpp" #else From 75548216cfb87de38822761829e001d46ecb537c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 10 May 2026 23:29:52 +0200 Subject: [PATCH 20/33] Add lyrical workflows, update README, and fix gcc-15 issues (#2344) --- .../workflows/rolling-abi-compatibility.yml | 4 +- .github/workflows/rolling-binary-build.yml | 4 +- ...ing-compatibility-lyrical-binary-build.yml | 37 +++++++++++++++++++ .github/workflows/rolling-debian-build.yml | 4 +- .github/workflows/rolling-pre-commit.yml | 2 +- .github/workflows/rolling-pre-release.yml | 2 +- .../rolling-rhel-semi-binary-build.yml | 4 +- .../workflows/rolling-semi-binary-build.yml | 6 +-- .../rolling-semi-binary-downstream-build.yml | 4 +- .github/workflows/rolling-source-build.yml | 6 +-- README.md | 3 +- .../test_pid_controller_dual_interface.cpp | 1 + ros2_controllers-not-released.lyrical.repos | 6 +++ ros2_controllers.lyrical.repos | 25 +++++++++++++ ros2_controllers_test_nodes/setup.py | 6 ++- ros_controls.lyrical.repos | 9 +++++ rqt_joint_trajectory_controller/setup.py | 6 ++- .../test_steering_controllers_library.cpp | 11 +++--- 18 files changed, 113 insertions(+), 27 deletions(-) create mode 100644 .github/workflows/rolling-compatibility-lyrical-binary-build.yml create mode 100644 ros2_controllers-not-released.lyrical.repos create mode 100644 ros2_controllers.lyrical.repos create mode 100644 ros_controls.lyrical.repos diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 20afed10b2..7beec22b7c 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -13,7 +13,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' - - 'ros2_controllers-not-released.kilted.repos' + - 'ros2_controllers-not-released.lyrical.repos' - '**.xml' concurrency: @@ -27,6 +27,6 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 61a201d097..9f73aba9a0 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -19,7 +19,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' - - 'ros2_controllers-not-released.kilted.repos' + - 'ros2_controllers-not-released.lyrical.repos' - '**.xml' push: *event schedule: @@ -37,7 +37,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] ROS_REPO: [main, testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-compatibility-lyrical-binary-build.yml b/.github/workflows/rolling-compatibility-lyrical-binary-build.yml new file mode 100644 index 0000000000..6e05bee98b --- /dev/null +++ b/.github/workflows/rolling-compatibility-lyrical-binary-build.yml @@ -0,0 +1,37 @@ +name: Check Rolling Compatibility on Lyrical +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Lyrical distro.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-lyrical-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + build-on-lyrical: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: lyrical + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 5032518403..59b8b18072 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -14,7 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' - - 'ros2_controllers.kilted.repos' + - 'ros2_controllers.lyrical.repos' - '**.xml' push: *event @@ -30,7 +30,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 54d7e90adf..7b3fb229bd 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -19,6 +19,6 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-pre-release.yml b/.github/workflows/rolling-pre-release.yml index 3550e647f1..1a4c506569 100644 --- a/.github/workflows/rolling-pre-release.yml +++ b/.github/workflows/rolling-pre-release.yml @@ -23,7 +23,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master with: diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 410e97e1db..e6d2ec9360 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -14,7 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' - - 'ros2_controllers.kilted.repos' + - 'ros2_controllers.lyrical.repos' - '**.xml' push: *event @@ -29,7 +29,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4be9be1678..2555b53aa0 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -18,7 +18,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' - - 'ros2_controllers.kilted.repos' + - 'ros2_controllers.lyrical.repos' - '**.xml' push: *event schedule: @@ -36,7 +36,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing @@ -47,7 +47,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml index 317164c4ed..e75136d476 100644 --- a/.github/workflows/rolling-semi-binary-downstream-build.yml +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -17,7 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.rolling.repos' - - 'ros_controls.kilted.repos' + - 'ros_controls.lyrical.repos' - '**.xml' concurrency: @@ -30,7 +30,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 34478d1ff7..52381e4450 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -14,7 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' - - 'ros2_controllers.kilted.repos' + - 'ros2_controllers.lyrical.repos' - '**.xml' pull_request: branches: @@ -31,8 +31,8 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [lyrical, rolling] with: ros_distro: ${{ matrix.ROS_DISTRO }} ref: master - container: ubuntu:24.04 + container: ubuntu:26.04 diff --git a/README.md b/README.md index a8927277bd..db407deed6 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,8 @@ If you are new to the project, please read the [contributing guide](https://cont ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_controllers__ubuntu_resolute_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_controllers__ubuntu_resolute_amd64/) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uR64__ros2_controllers__ubuntu_resolute_amd64__binary)](https://build.ros2.org/job/Rbin_uR64__ros2_controllers__ubuntu_resolute_amd64__binary/) +**Lyrical** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | see above
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Ldev__ros2_controllers__ubuntu_resolute_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Ldev__ros2_controllers__ubuntu_resolute_amd64/) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Lbin_uR64__ros2_controllers__ubuntu_resolute_amd64__binary)](https://build.ros2.org/job/Lbin_uR64__ros2_controllers__ubuntu_resolute_amd64__binary/) **Kilted** | [`kilted`](https://github.com/ros-controls/ros2_controllers/tree/kilted) | [![Kilted Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-binary-build.yml?branch=master)
[![Kilted Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/kilted/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) **Jazzy** | [`jazzy`](https://github.com/ros-controls/ros2_controllers/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ros2_controllers__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ros2_controllers__ubuntu_jammy_amd64/) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__ros2_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__ros2_controllers__ubuntu_jammy_amd64__binary/) diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 75eb531863..f69acfa2d4 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -35,6 +35,7 @@ class PidControllerDualInterfaceTest : public PidControllerFixture: +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers.lyrical.repos b/ros2_controllers.lyrical.repos new file mode 100644 index 0000000000..067271620f --- /dev/null +++ b/ros2_controllers.lyrical.repos @@ -0,0 +1,25 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + ros2_control_cmake: + type: git + url: https://github.com/ros-controls/ros2_control_cmake.git + version: master diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index c2fea497df..da2b2112bd 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -44,7 +44,11 @@ long_description="""\ Demo nodes for showing and testing functionalities of the ros2_control framework.""", license="Apache License, Version 2.0", - tests_require=["pytest"], + extras_require={ + "test": [ + "pytest", + ], + }, entry_points={ "console_scripts": [ "publisher_forward_position_controller = \ diff --git a/ros_controls.lyrical.repos b/ros_controls.lyrical.repos new file mode 100644 index 0000000000..da64a510fd --- /dev/null +++ b/ros_controls.lyrical.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 3860e17c10..0bcf44a93c 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -36,7 +36,11 @@ maintainer="Bence Magyar", maintainer_email="bence.magyar.robotics@gmail.com", license="Apache License, Version 2.0", - tests_require=["pytest"], + extras_require={ + "test": [ + "pytest", + ], + }, entry_points={ "console_scripts": [ "rqt_joint_trajectory_controller = \ diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 275e44c136..d597262d68 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -406,11 +406,10 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) const rclcpp::Duration period = rclcpp::Duration::from_seconds(dt); rclcpp::Time test_time = controller_->get_node()->now(); - auto move_robot = [&](double vx, double vy, double wz) + auto move_robot = [&](double vx, double wz) { - controller_->reference_interfaces_[0] = vx; // linear x - controller_->reference_interfaces_[1] = vy; // linear y - controller_->reference_interfaces_[2] = wz; // angular z + controller_->reference_interfaces_[0] = vx; // linear velocity + controller_->reference_interfaces_[1] = wz; // angular velocity ASSERT_EQ(controller_->update(test_time, period), controller_interface::return_type::OK); @@ -423,7 +422,7 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) }; // 1. Test robot movement initially - for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0); + for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0); ASSERT_GT(controller_->odometry_.get_x(), 0.0); move_robot(0.0, 0.0, 0.0); @@ -446,7 +445,7 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) // 3. Move forward again to verify double start_y = controller_->odometry_.get_y(); - for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0); // we are facing +Y now + for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0); // we are facing +Y now EXPECT_GT(controller_->odometry_.get_y(), start_y); } From 5d808d619019ed1da32e3ff318825b52b310f395 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 12 May 2026 11:20:25 +0000 Subject: [PATCH 21/33] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 7 +++++++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ chained_filter_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gpio_controllers/CHANGELOG.rst | 3 +++ gps_sensor_broadcaster/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 8 ++++++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ mecanum_drive_controller/CHANGELOG.rst | 5 +++++ motion_primitives_controllers/CHANGELOG.rst | 3 +++ omni_wheel_drive_controller/CHANGELOG.rst | 6 ++++++ parallel_gripper_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 6 ++++++ pose_broadcaster/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ state_interfaces_broadcaster/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 7 +++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ 26 files changed, 140 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 55cb6a40ec..1fe7efbbd8 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 2936268c72..235745f631 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Suppress cppcheck errors from macros from version.h (`#2346 `_) +* fix: correct ASSERT_EQ to ASSERT_NE in admittance controller load test (`#2264 `_) +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich, Souri Rishik + 6.6.0 (2026-04-22) ------------------ * Update admittance_controller to use shared 6D robot description (`#2173 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index e5c939407f..c58a3951b4 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/chained_filter_controller/CHANGELOG.rst b/chained_filter_controller/CHANGELOG.rst index 65ec96cb03..fb60b5f3e1 100644 --- a/chained_filter_controller/CHANGELOG.rst +++ b/chained_filter_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chained_filter_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index d65c4b5d21..5a36329dfe 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove deprecated publish_rate from diff_drive_controller (`#2259 `_) +* Remove deprecated odometry reset methods (`#2252 `_) +* Bump C++ version to C++20 (`#2331 `_) +* diff_drive: set parameters as read_only or dynamically update them (`#2293 `_) +* Contributors: Bhavin Umatiya, Christoph Fröhlich, Ege Kural, kamal2730 + 6.6.0 (2026-04-22) ------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 760e04d68a..bff928f8c5 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Suppress cppcheck errors from macros from version.h (`#2346 `_) +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 5507ad829d..09de0595fd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index d4b0ca05e4..2a950fc525 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.6.0 (2026-04-22) ------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst index 685b4dff11..344b1bc43d 100644 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gps_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ * Update controller tests to use configure/activate instead of on_configure/on_activate (`#1682 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5ffebbd7c6..6e645ea225 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 11dde2519d..94a940bdd0 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Suppress cppcheck errors from macros from version.h (`#2346 `_) +* added warning messages when unable to remap the interface properly in Joint State Broadcaster. (`#2082 `_) +* Bump C++ version to C++20 (`#2331 `_) +* fix(joint_state_broadcaster): suppress confusing warning for standard interfaces (`#2276 `_) +* Contributors: Christoph Fröhlich, Maksim Sviridov, Mithun Chakladar + 6.6.0 (2026-04-22) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c692389f90..2f5d9205fa 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Suppress cppcheck errors from macros from version.h (`#2346 `_) +* Bump C++ version to C++20 (`#2331 `_) +* fix JTC userdoc YAML indentation and stray quote (`#2327 `_) +* Contributors: Christoph Fröhlich, ahmedbilal9 + 6.6.0 (2026-04-22) ------------------ * Fix segfault in jtc if joint name not in urdf (`#2321 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index ae43e15c3b..bae7463e91 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/motion_primitives_controllers/CHANGELOG.rst b/motion_primitives_controllers/CHANGELOG.rst index fc4782b0bd..d7b7945b5f 100644 --- a/motion_primitives_controllers/CHANGELOG.rst +++ b/motion_primitives_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package motion_primitives_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.6.0 (2026-04-22) ------------------ diff --git a/omni_wheel_drive_controller/CHANGELOG.rst b/omni_wheel_drive_controller/CHANGELOG.rst index 7d4c33a472..bbc9f20c8f 100644 --- a/omni_wheel_drive_controller/CHANGELOG.rst +++ b/omni_wheel_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package omni_wheel_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove deprecated odometry reset methods (`#2252 `_) +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich, Ege Kural + 6.6.0 (2026-04-22) ------------------ diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 252c7c6f82..daf149f259 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ * fix(parallel_gripper): rename variables for consistency (`#2314 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 45022e9484..7d0addc2ab 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index f593778287..64b9a90971 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 1930191996..43d8b0c310 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 6.6.0 (2026-04-22) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 917414441c..acc160c221 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove deprecated controller specializations (`#2016 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 730862a4b1..48b41d2db9 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 35661b6474..193aca47b2 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) +* [rqt_jtc] Use urdf_parser_py (`#2254 `_) +* Contributors: Christoph Fröhlich, Sahil Lakhmani + 6.6.0 (2026-04-22) ------------------ * [RQT_JTC] add unit tests for parse_joint_limits (`#2281 `_) diff --git a/state_interfaces_broadcaster/CHANGELOG.rst b/state_interfaces_broadcaster/CHANGELOG.rst index d51d6dd84c..358709b52f 100644 --- a/state_interfaces_broadcaster/CHANGELOG.rst +++ b/state_interfaces_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package state_interfaces_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index cb5843de4b..8200bb015b 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) +* Remove deprecated odometry reset methods (`#2252 `_) +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich, Ege Kural + 6.6.0 (2026-04-22) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 9bab8a2664..e8d4ddb244 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index fbd4bf7489..b7786ac32f 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bump C++ version to C++20 (`#2331 `_) +* Contributors: Christoph Fröhlich + 6.6.0 (2026-04-22) ------------------ From 567c1a6ebe3e704438a2429feacd9d68e21e8f97 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 12 May 2026 11:20:34 +0000 Subject: [PATCH 22/33] 6.7.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- chained_filter_controller/CHANGELOG.rst | 4 ++-- chained_filter_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gps_sensor_broadcaster/CHANGELOG.rst | 4 ++-- gps_sensor_broadcaster/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- motion_primitives_controllers/CHANGELOG.rst | 4 ++-- motion_primitives_controllers/package.xml | 2 +- omni_wheel_drive_controller/CHANGELOG.rst | 4 ++-- omni_wheel_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- state_interfaces_broadcaster/CHANGELOG.rst | 4 ++-- state_interfaces_broadcaster/package.xml | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- 54 files changed, 80 insertions(+), 80 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 1fe7efbbd8..a8b4819507 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index e200dbdc6d..95182a9575 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 6.6.0 + 6.7.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 235745f631..cd545a58b4 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Suppress cppcheck errors from macros from version.h (`#2346 `_) * fix: correct ASSERT_EQ to ASSERT_NE in admittance controller load test (`#2264 `_) * Bump C++ version to C++20 (`#2331 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 95e32b817e..e22c0b2b08 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 6.6.0 + 6.7.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index c58a3951b4..0ebd4b40de 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 27bdb7051e..e44361e077 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 6.6.0 + 6.7.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/chained_filter_controller/CHANGELOG.rst b/chained_filter_controller/CHANGELOG.rst index fb60b5f3e1..8e73ff7bbf 100644 --- a/chained_filter_controller/CHANGELOG.rst +++ b/chained_filter_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package chained_filter_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/chained_filter_controller/package.xml b/chained_filter_controller/package.xml index ba2f9d4c8d..aa54f4e8fc 100644 --- a/chained_filter_controller/package.xml +++ b/chained_filter_controller/package.xml @@ -2,7 +2,7 @@ chained_filter_controller - 6.6.0 + 6.7.0 ros2_controller for configuring filter chains Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5a36329dfe..e4a9934cec 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Remove deprecated publish_rate from diff_drive_controller (`#2259 `_) * Remove deprecated odometry reset methods (`#2252 `_) * Bump C++ version to C++20 (`#2331 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 93ec3398b9..00bec46d75 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 6.6.0 + 6.7.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index bff928f8c5..dabd211dd2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Suppress cppcheck errors from macros from version.h (`#2346 `_) * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 3c6e094987..be13e6d551 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 6.6.0 + 6.7.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 09de0595fd..5db9efdb27 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index eae9e01517..acd2566a11 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 6.6.0 + 6.7.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 2a950fc525..54f3672d15 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ 6.6.0 (2026-04-22) ------------------ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 443c0db0c0..47a82ac5a1 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 6.6.0 + 6.7.0 Controllers to interact with gpios. Bence Magyar diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst index 344b1bc43d..dbfb018b7d 100644 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gps_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml index 9204eb10ab..ca460a466e 100644 --- a/gps_sensor_broadcaster/package.xml +++ b/gps_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ gps_sensor_broadcaster - 6.6.0 + 6.7.0 Controller to publish readings of GPS sensors. Apache License 2.0 diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6e645ea225..5dcafb7348 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 89d75464fc..4f606cb80a 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 6.6.0 + 6.7.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 94a940bdd0..6dc9068746 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Suppress cppcheck errors from macros from version.h (`#2346 `_) * added warning messages when unable to remap the interface properly in Joint State Broadcaster. (`#2082 `_) * Bump C++ version to C++20 (`#2331 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 706a899383..ddcfe29f26 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 6.6.0 + 6.7.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2f5d9205fa..43dc427862 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Suppress cppcheck errors from macros from version.h (`#2346 `_) * Bump C++ version to C++20 (`#2331 `_) * fix JTC userdoc YAML indentation and stray quote (`#2327 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f9540a4da6..4c7f490235 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 6.6.0 + 6.7.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index bae7463e91..de016d049f 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index f9493add71..0c3200dc2a 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 6.6.0 + 6.7.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/motion_primitives_controllers/CHANGELOG.rst b/motion_primitives_controllers/CHANGELOG.rst index d7b7945b5f..cd37e46f3f 100644 --- a/motion_primitives_controllers/CHANGELOG.rst +++ b/motion_primitives_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package motion_primitives_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ 6.6.0 (2026-04-22) ------------------ diff --git a/motion_primitives_controllers/package.xml b/motion_primitives_controllers/package.xml index d1d56f3669..7ba723cfb2 100644 --- a/motion_primitives_controllers/package.xml +++ b/motion_primitives_controllers/package.xml @@ -2,7 +2,7 @@ motion_primitives_controllers - 6.6.0 + 6.7.0 Package to control robots using motion primitives like PTP, LIN and CIRC Bence Magyar diff --git a/omni_wheel_drive_controller/CHANGELOG.rst b/omni_wheel_drive_controller/CHANGELOG.rst index bbc9f20c8f..6441adb975 100644 --- a/omni_wheel_drive_controller/CHANGELOG.rst +++ b/omni_wheel_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package omni_wheel_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Remove deprecated odometry reset methods (`#2252 `_) * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich, Ege Kural diff --git a/omni_wheel_drive_controller/package.xml b/omni_wheel_drive_controller/package.xml index ed416b4e31..fe8a92cbf3 100644 --- a/omni_wheel_drive_controller/package.xml +++ b/omni_wheel_drive_controller/package.xml @@ -2,7 +2,7 @@ omni_wheel_drive_controller - 6.6.0 + 6.7.0 A chainable controller for mobile robots with omnidirectional drive with three or more omni wheels. Apache License 2.0 diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index daf149f259..54a8ec5e54 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index f766fc08fe..0e7e68361f 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 6.6.0 + 6.7.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 7d0addc2ab..62b08ca34b 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 52a675905b..dcf334e288 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 6.6.0 + 6.7.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 64b9a90971..b522d2e4fd 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 9a3052370e..fa5302cbb1 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 6.6.0 + 6.7.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 43d8b0c310..d833d2259c 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ 6.6.0 (2026-04-22) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 284db6d277..03b14eadec 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 6.6.0 + 6.7.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index acc160c221..da8bdc494c 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Remove deprecated controller specializations (`#2016 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5e0b0b1346..77f1264208 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 6.6.0 + 6.7.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 48b41d2db9..d020d78db1 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index fe7748f27b..34c2006064 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 6.6.0 + 6.7.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index da2b2112bd..9be847b0b4 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="6.6.0", + version="6.7.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 193aca47b2..aba939a469 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) * [rqt_jtc] Use urdf_parser_py (`#2254 `_) * Contributors: Christoph Fröhlich, Sahil Lakhmani diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index bd6ee64276..458a5a2b5c 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 6.6.0 + 6.7.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 0bcf44a93c..bd608ff7c5 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="6.6.0", + version="6.7.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/state_interfaces_broadcaster/CHANGELOG.rst b/state_interfaces_broadcaster/CHANGELOG.rst index 358709b52f..9b7ffdc8af 100644 --- a/state_interfaces_broadcaster/CHANGELOG.rst +++ b/state_interfaces_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package state_interfaces_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/state_interfaces_broadcaster/package.xml b/state_interfaces_broadcaster/package.xml index 11de13f972..b95534613b 100644 --- a/state_interfaces_broadcaster/package.xml +++ b/state_interfaces_broadcaster/package.xml @@ -1,7 +1,7 @@ state_interfaces_broadcaster - 6.6.0 + 6.7.0 Broadcaster to publish the requested hardware interface states Bence Magyar diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8200bb015b..471a87ea2c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Add lyrical workflows, update README, and fix gcc-15 issues (`#2344 `_) * Remove deprecated odometry reset methods (`#2252 `_) * Bump C++ version to C++20 (`#2331 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 98a0bd48fb..ceab56e0cb 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 6.6.0 + 6.7.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index e8d4ddb244..515e44827e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index b30e1f5d01..3455453f56 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 6.6.0 + 6.7.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index b7786ac32f..6b3f8b5f17 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +6.7.0 (2026-05-12) +------------------ * Bump C++ version to C++20 (`#2331 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7e45ab1d36..791e4a663d 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 6.6.0 + 6.7.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar From 7fdd9ea41e9551cbf0190affb0cece06ad55bcb1 Mon Sep 17 00:00:00 2001 From: "Peter Mitrano (AR)" Date: Thu, 14 May 2026 12:50:19 +0200 Subject: [PATCH 23/33] More general initialization of state from command (#2294) --- doc/release_notes.rst | 2 + .../joint_trajectory_controller.hpp | 11 +- .../src/joint_trajectory_controller.cpp | 199 ++++++------------ ...oint_trajectory_controller_parameters.yaml | 2 +- .../test/test_trajectory_controller.cpp | 105 +++++---- 5 files changed, 139 insertions(+), 180 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5c9e7645a4..df3986cd6b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -41,6 +41,8 @@ omni_wheel_drive_controller joint_trajectory_controller *************************** +* When using ``set_last_command_interface_value_as_state_on_activation``, it is no longer required to have state and command for the same interface type (e.g. velocity). With this param set, the JTC state and command will be initialized using a command interface value, if available, and will otherwise fallback to the value read from the state interface. This allows you to have position command and position+velocity state, for example, which previously would have been disallowed (with this param set). (`#2294 + `_) * Fill in 0 velocities and accelerations into point before trajectories if the state interfaces don't contain velocity / acceleration information, but the trajectory does. This way, the segment up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043 diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 7749d84864..58898bbab8 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -273,12 +273,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. - * This is only possible if command AND state interfaces exist for the same type, - * therefore needs check for both. + * state values (e.g. velocity, acceleration, or effort) which do not have command interfaces will + * NOT be updated. * @param[out] state to be filled with values from command interfaces. - * @return true if all interfaces exists and contain non-NaN values, false otherwise. */ - bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + void update_state_from_command_interfaces(JointTrajectoryPoint & state); bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); void query_state_service( @@ -296,6 +295,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0); + void assign_point_from_command_interface( + std::vector & trajectory_point_interface, + const std::vector> & + joint_interface); /** * @brief Set scaling factor used for speed scaling trajectory execution diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c2474bbfdb..5e6e1e00b4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -44,6 +44,18 @@ namespace joint_trajectory_controller { + +auto interface_has_values = [](const auto & joint_interface) -> bool +{ + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { + auto interface_op = interface.get().get_optional(); + return !interface_op.has_value() || !std::isfinite(interface_op.value()); + }) == joint_interface.end(); +}; + JointTrajectoryController::JointTrajectoryController() : controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0) { @@ -599,27 +611,6 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory } } }; - auto assign_point_from_command_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - std::fill( - trajectory_point_interface.begin(), trajectory_point_interface.end(), - std::numeric_limits::quiet_NaN()); - for (size_t index = 0; index < num_cmd_joints_; ++index) - { - const auto joint_command_interface_value_op = joint_interface[index].get().get_optional(); - if (!joint_command_interface_value_op.has_value()) - { - RCLCPP_DEBUG( - logger, "Unable to retrieve joint command interface value for joint at index %zu", index); - } - else - { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_command_interface_value_op.value(); - } - } - }; // Assign values from the hardware // Position states always exist @@ -652,101 +643,41 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory } } -bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) +void JointTrajectoryController::update_state_from_command_interfaces(JointTrajectoryPoint & state) { - bool has_values = true; - - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - for (size_t index = 0; index < num_cmd_joints_; ++index) - { - const auto joint_interface_value_op = joint_interface[index].get().get_optional(); - if (!joint_interface_value_op.has_value()) - { - RCLCPP_DEBUG( - get_node()->get_logger(), - "Unable to retrieve value of joint interface for joint at index %zu", index); - } - else - { - trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value(); - } - } - }; - - auto interface_has_values = [](const auto & joint_interface) - { - return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) - { - auto interface_op = interface.get().get_optional(); - return !interface_op.has_value() || std::isnan(interface_op.value()); - }) == joint_interface.end(); - }; - - // Assign values from the command interfaces as state. Therefore needs check for both. - // Position state interface has to exist always + // Assign values from the command interfaces as state + // Position state interface has to exist always, so no need to check for it if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) { - assign_point_from_interface(state.positions, joint_command_interface_[0]); - } - else - { - state.positions.clear(); - has_values = false; + assign_point_from_command_interface(state.positions, joint_command_interface_[0]); } + // velocity and acceleration states are optional - if (has_velocity_state_interface_) + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) { - if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) + // If no state interface exists, then our velocities vector will be empty, and we must resize + // before assigning. + if (!has_velocity_state_interface_) { - assign_point_from_interface(state.velocities, joint_command_interface_[1]); + state.velocities.resize(dof_, std::numeric_limits::quiet_NaN()); } - else - { - state.velocities.clear(); - has_values = false; - } - } - else - { - state.velocities.clear(); + assign_point_from_command_interface(state.velocities, joint_command_interface_[1]); } - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) + + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) { - if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) - { - assign_point_from_interface(state.accelerations, joint_command_interface_[2]); - } - else + if (!has_acceleration_state_interface_) { - state.accelerations.clear(); - has_values = false; + state.accelerations.resize(dof_, std::numeric_limits::quiet_NaN()); } - } - else - { - state.accelerations.clear(); + assign_point_from_command_interface(state.accelerations, joint_command_interface_[2]); } - // Effort state always comes from last command - if (has_effort_command_interface_) + if (has_effort_command_interface_ && interface_has_values(joint_command_interface_[3])) { - if (interface_has_values(joint_command_interface_[3])) - { - assign_point_from_interface(state.effort, joint_command_interface_[3]); - } - else - { - state.effort.clear(); - has_values = false; - } + state.effort.resize(dof_, std::numeric_limits::quiet_NaN()); + assign_point_from_command_interface(state.effort, joint_command_interface_[3]); } - - return has_values; } bool JointTrajectoryController::read_commands_from_command_interfaces( @@ -771,17 +702,6 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( } }; - auto interface_has_values = [](const auto & joint_interface) - { - return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) - { - auto interface_op = interface.get().get_optional(); - return !interface_op.has_value() || std::isnan(interface_op.value()); - }) == joint_interface.end(); - }; - // Assign values from the command interfaces as command. if (has_position_command_interface_) { @@ -1280,28 +1200,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; - // Handle restart of controller by reading from commands if those are not NaN (a controller was - // running already) - trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, dof_); - // read from cmd joints only if all joints have command interface - // otherwise it leaves the entries of joints without command interface NaN. - // if no interpolate_from_desired_state, state_current_ is then used for - // `set_point_before_trajectory_msg` and future trajectory sampling will always give NaN for these - // joints - if ( - params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_ && - read_state_from_command_interfaces(state)) - { - state_current_ = state; - last_commanded_state_ = state; - } - else + // Initialize current state storage from hardware state interfaces + read_state_from_state_interfaces(state_current_); + read_state_from_state_interfaces(last_commanded_state_); + + if (params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_) { - // Initialize current state storage from hardware - read_state_from_state_interfaces(state_current_); - read_state_from_state_interfaces(last_commanded_state_); + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) + // The function checks if all joints have values on the command interfaces. + // otherwise it will not update them, leaving them based on the state interfaces. + update_state_from_command_interfaces(state_current_); + update_state_from_command_interfaces(last_commanded_state_); } + // reset/zero out all of the PID's (The integral term is not retained and reset to zero) for (auto & pid : pids_) { @@ -2165,6 +2077,31 @@ void JointTrajectoryController::init_hold_position_msg() } } +void JointTrajectoryController::assign_point_from_command_interface( + std::vector & trajectory_point_interface, + const std::vector> & + joint_interface) +{ + std::fill( + trajectory_point_interface.begin(), trajectory_point_interface.end(), + std::numeric_limits::quiet_NaN()); + for (size_t index = 0; index < num_cmd_joints_; ++index) + { + const auto joint_command_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve joint command interface value for joint at index %zu", index); + } + else + { + trajectory_point_interface.at(map_cmd_to_joints_[index]) = + joint_command_interface_value_op.value(); + } + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index f2c4c9e112..65dd38d313 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -100,7 +100,7 @@ joint_trajectory_controller: set_last_command_interface_value_as_state_on_activation: { type: bool, default_value: true, - description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. If an interface (e.g. velocity) is available only as state and not command, the state is used. Use this to prevent sagging/jumps on activation when using soft or compliant hardware. When set to false, the current state is used for both.", } action_monitor_rate: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index c37372e3e8..6e0c0a98f1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -2385,62 +2385,79 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd); // no call of update method, so the values should be read from command interfaces - auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); for (size_t i = 0; i < 3; ++i) { - // check position if (traj_controller_->has_position_command_interface()) { - // check velocity - if (traj_controller_->has_velocity_state_interface()) - { - if (traj_controller_->has_velocity_command_interface()) - { - // check acceleration - if (traj_controller_->has_acceleration_state_interface()) - { - if (traj_controller_->has_acceleration_command_interface()) - { - // should have set it to last position + velocity + acceleration command - EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); - EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); - } - else - { - // should have set it to the state interface instead - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); - EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); - } - } - else - { - // should have set it to last position + velocity command - EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); - } - } - else - { - // should have set it to the state interface instead - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); - } - } - else - { - // should have set it to last position command - EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); - } + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); } else { // should have set it to the state interface instead EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + } + else if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + } + else if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + + executor.cancel(); +} + +// Testing the behavior when set_last_command_interface_value_as_state_on_activation is false. +// On activation, both command and state should be equal to the current state values +TEST_P(TrajectoryControllerTestParameterized, test_set_last_command_interface_on_activation_false) +{ + rclcpp::Parameter const set_last_command_on_activation( + "set_last_command_interface_value_as_state_on_activation", false); + + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; + for (size_t i = 0; i < 3; ++i) + { + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); + } + + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController( + executor, {set_last_command_on_activation}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (size_t i = 0; i < 3; ++i) + { + EXPECT_EQ(current_state_when_offset.positions[i], INITIAL_POS_JOINTS[i]); + + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(current_state_when_offset.velocities[i], INITIAL_VEL_JOINTS[i]); + } + + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(current_state_when_offset.accelerations[i], INITIAL_ACC_JOINTS[i]); + } } executor.cancel(); From 5e0f88ca7004d8b71f680ec73006b4ebee77c5ac Mon Sep 17 00:00:00 2001 From: Junius Santoso <9374081+jsantoso91@users.noreply.github.com> Date: Thu, 14 May 2026 09:16:43 -0500 Subject: [PATCH 24/33] Test fix - call appropriate lifecycle transitions in controller tests: admittance_controller, pose_broadcaster, tricycle_steering_controller (#2345) --- .../test/test_admittance_controller.cpp | 50 +++++++++---------- .../test/test_admittance_controller.hpp | 6 +++ .../test/test_pose_broadcaster.cpp | 40 ++++----------- .../test/test_pose_broadcaster.hpp | 6 +++ .../test_tricycle_steering_controller.cpp | 38 +++++++------- .../test_tricycle_steering_controller.hpp | 6 +++ ...tricycle_steering_controller_preceding.cpp | 4 +- 7 files changed, 74 insertions(+), 76 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index ba89d088e3..6f47744a03 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -79,7 +79,7 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); @@ -156,7 +156,7 @@ TEST_F(AdmittanceControllerTest, check_interfaces) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); @@ -200,17 +200,17 @@ TEST_F(AdmittanceControllerTest, activate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(activate_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, missing_pos_state_interface) { auto overrides = {rclcpp::Parameter("state_interfaces", std::vector{"velocity"})}; SetUpController("test_admittance_controller", overrides); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); + ASSERT_FALSE(configure_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, only_vel_command_interface) @@ -218,8 +218,8 @@ TEST_F(AdmittanceControllerTest, only_vel_command_interface) command_interface_types_ = {"velocity"}; auto overrides = {rclcpp::Parameter("command_interfaces", std::vector{"velocity"})}; SetUpController("test_admittance_controller", overrides); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -230,7 +230,7 @@ TEST_F(AdmittanceControllerTest, only_pos_reference_interface) auto overrides = { rclcpp::Parameter("chainable_command_interfaces", std::vector{"position"})}; SetUpController("test_admittance_controller", overrides); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, only_vel_reference_interface) @@ -238,7 +238,7 @@ TEST_F(AdmittanceControllerTest, only_vel_reference_interface) auto overrides = { rclcpp::Parameter("chainable_command_interfaces", std::vector{"velocity"})}; SetUpController("test_admittance_controller", overrides); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, invalid_reference_interface) @@ -246,15 +246,15 @@ TEST_F(AdmittanceControllerTest, invalid_reference_interface) auto overrides = {rclcpp::Parameter( "chainable_command_interfaces", std::vector{"invalid_interface"})}; SetUpController("test_admittance_controller", overrides); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + ASSERT_FALSE(configure_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, update_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); broadcast_tfs(); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -265,20 +265,20 @@ TEST_F(AdmittanceControllerTest, deactivate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); } TEST_F(AdmittanceControllerTest, reactivate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); assign_interfaces(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(activate_succeeds(controller_)); broadcast_tfs(); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -289,8 +289,8 @@ TEST_F(AdmittanceControllerTest, publish_status_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); broadcast_tfs(); ASSERT_EQ( @@ -325,8 +325,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); broadcast_tfs(); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -360,8 +360,8 @@ TEST_F(AdmittanceControllerTest, check_frame_ids_in_controller_state) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); broadcast_tfs(); // force torque sensor diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index c46e0b782a..40832ef4a5 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -31,6 +31,7 @@ #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/test_utils.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -51,6 +52,11 @@ using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; +using controller_interface::activate_succeeds; +using controller_interface::cleanup_succeeds; +using controller_interface::configure_succeeds; +using controller_interface::deactivate_succeeds; + namespace { const double COMMON_THRESHOLD = 0.001; diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 737661ac78..3942fcdea8 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -56,9 +56,7 @@ TEST_F(PoseBroadcasterTest, Configure_Success) pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // Configure controller - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(configure_succeeds(pose_broadcaster_)); // Verify command interface configuration const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); @@ -81,12 +79,8 @@ TEST_F(PoseBroadcasterTest, Activate_Success) pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // Configure and activate controller - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(configure_succeeds(pose_broadcaster_)); + ASSERT_TRUE(activate_succeeds(pose_broadcaster_)); // Verify command and state interface configuration { @@ -102,9 +96,7 @@ TEST_F(PoseBroadcasterTest, Activate_Success) } // Deactivate controller - ASSERT_EQ( - pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(deactivate_succeeds(pose_broadcaster_)); // Verify command and state interface configuration { @@ -129,12 +121,8 @@ TEST_F(PoseBroadcasterTest, Update_Success) pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // Configure and activate controller - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(configure_succeeds(pose_broadcaster_)); + ASSERT_TRUE(activate_succeeds(pose_broadcaster_)); ASSERT_EQ( pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), @@ -154,12 +142,8 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); // Configure and activate controller - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(configure_succeeds(pose_broadcaster_)); + ASSERT_TRUE(activate_succeeds(pose_broadcaster_)); // Subscribe to pose topic geometry_msgs::msg::PoseStamped pose_msg; @@ -205,12 +189,8 @@ TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); // Configure and activate controller - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::SUCCESS); + ASSERT_TRUE(configure_succeeds(pose_broadcaster_)); + ASSERT_TRUE(activate_succeeds(pose_broadcaster_)); ASSERT_TRUE(pose_position_x_->set_value(std::numeric_limits::quiet_NaN())); diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index 65b655a1ce..782b5d6eb0 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -20,12 +20,18 @@ #include #include +#include "controller_interface/test_utils.hpp" #include "rclcpp/executors.hpp" #include "pose_broadcaster/pose_broadcaster.hpp" using pose_broadcaster::PoseBroadcaster; +using controller_interface::activate_succeeds; +using controller_interface::cleanup_succeeds; +using controller_interface::configure_succeeds; +using controller_interface::deactivate_succeeds; + class PoseBroadcasterTest : public ::testing::Test { public: diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 115f5f6eee..5b3e8b00e5 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); ASSERT_THAT( controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_)); @@ -46,7 +46,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); @@ -92,8 +92,8 @@ TEST_F(TricycleSteeringControllerTest, activate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); // check that the message is reset auto msg = controller_->input_ref_.get(); @@ -109,8 +109,8 @@ TEST_F(TricycleSteeringControllerTest, update_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -121,20 +121,20 @@ TEST_F(TricycleSteeringControllerTest, deactivate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); } TEST_F(TricycleSteeringControllerTest, reactivate_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( @@ -148,9 +148,9 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); controller_->set_chained_mode(false); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically @@ -188,9 +188,9 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_TRUE(controller_->is_in_chained_mode()); controller_->reference_interfaces_[0] = 0.1; @@ -225,8 +225,8 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 895039978a..8594b6a8f0 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -23,6 +23,7 @@ #include #include +#include "controller_interface/test_utils.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "rclcpp/executor.hpp" @@ -46,6 +47,11 @@ using tricycle_steering_controller::CMD_STEER_WHEEL; using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; +using controller_interface::activate_succeeds; +using controller_interface::cleanup_succeeds; +using controller_interface::configure_succeeds; +using controller_interface::deactivate_succeeds; + namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp index 5c9210ae70..f730e74b53 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp @@ -27,7 +27,7 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); ASSERT_THAT( controller_->params_.traction_joints_names, @@ -47,7 +47,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(configure_succeeds(controller_)); auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); From a8bfbb035f22e24e79767bde1f270a3041f58854 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 May 2026 18:25:40 +0200 Subject: [PATCH 25/33] [JSB] Remove dynamic_joint_states and use INDIVIDUAL_BEST_EFFORT for interfaces (#2187) --- joint_state_broadcaster/CMakeLists.txt | 2 - joint_state_broadcaster/doc/userdoc.rst | 28 +--- .../joint_state_broadcaster.hpp | 18 +-- joint_state_broadcaster/package.xml | 1 - .../src/joint_state_broadcaster.cpp | 151 +++++++----------- .../joint_state_broadcaster_parameters.yaml | 10 +- .../test/test_joint_state_broadcaster.cpp | 89 ++++++----- .../test/test_joint_state_broadcaster.hpp | 34 ++++ 8 files changed, 151 insertions(+), 182 deletions(-) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 015079b00f..79cefe2d2b 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -7,7 +7,6 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces - control_msgs controller_interface generate_parameter_library pluginlib @@ -43,7 +42,6 @@ target_link_libraries(joint_state_broadcaster PUBLIC realtime_tools::realtime_tools urdf::urdf ${sensor_msgs_TARGETS} - ${control_msgs_TARGETS} ${builtin_interfaces_TARGETS}) pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index a66ba98bb0..9f4251d6c0 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -5,7 +5,7 @@ joint_state_broadcaster ======================= -The broadcaster reads all state interfaces and reports them on ``/joint_states`` and ``/dynamic_joint_states``. +The broadcaster reads all state interfaces and reports them on ``/joint_states``. Commands -------- @@ -29,31 +29,11 @@ Published topics and ``effort`` — for joints that provide them. If a joint does not expose a given movement interface, that field is omitted/left empty for that joint in the message. -* ``/dynamic_joint_states`` (``control_msgs/msg/DynamicJointState``): - - Publishes **all available state interfaces** for each joint. This includes the - movement interfaces (position/velocity/effort) *and* any additional or custom - interfaces provided by the hardware (e.g., temperature, voltage, torque sensor - readings, calibration flags). - - The message maps ``joint_names`` to per-joint interface name lists and values. - Example payload:: - - joint_names: [joint1, joint2] - interface_values: - - interface_names: [position, velocity, effort] - values: [1.5708, 0.0, 0.20] - - interface_names: [position, temperature] - values: [0.7854, 42.1] - - Use this topic if you need *every* reported interface, not just movement. - .. note:: - If ``use_local_topics`` is set to ``true``, both topics are published in the - controller’s namespace (e.g., ``/my_state_broadcaster/joint_states`` and - ``/my_state_broadcaster/dynamic_joint_states``). If ``false`` (default), - they are published at the root (e.g., ``/joint_states``). + If ``use_local_topics`` is set to ``true``, the topic is published in the + controller’s namespace (e.g., ``/my_state_broadcaster/joint_states``). If ``false`` (default), + it is published at the root (e.g., ``/joint_states``). Parameters diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 7e344bf272..9618535896 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -20,7 +20,6 @@ #include #include -#include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -59,8 +58,6 @@ namespace joint_state_broadcaster * Publishes to: * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement * (position, velocity, effort). - * - \b dynamic_joint_states (control_msgs::msg::DynamicJointState): Joint states regardless of - * its interface type. */ class JointStateBroadcaster : public controller_interface::ControllerInterface { @@ -89,9 +86,13 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface bool init_joint_data(); void init_auxiliary_data(); void init_joint_state_msg(); - void init_dynamic_joint_state_msg(); + + [[deprecated( + "use_all_available_interfaces is deprecated. Use use_urdf_joint_interfaces method instead")]] bool use_all_available_interfaces() const; + bool use_urdf_joint_interfaces() const; + protected: // Optional parameters std::shared_ptr param_listener_; @@ -108,14 +109,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface realtime_joint_state_publisher_; sensor_msgs::msg::JointState joint_state_msg_; - // For the DynamicJointState format, we use a map to buffer values in for easier lookup - // This allows to preserve whatever order or names/interfaces were initialized. std::unordered_map> name_if_value_mapping_; - std::shared_ptr> - dynamic_joint_state_publisher_; - std::shared_ptr> - realtime_dynamic_joint_state_publisher_; - control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; urdf::Model model_; bool is_model_loaded_ = false; @@ -135,8 +129,6 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface }; std::vector joint_states_data_; - - std::vector> dynamic_joint_states_data_; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index ddcfe29f26..484ba9dd73 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -24,7 +24,6 @@ backward_ros builtin_interfaces - control_msgs controller_interface generate_parameter_library pluginlib diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index d93ee6d67e..1e71f38908 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -68,9 +68,22 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf { controller_interface::InterfaceConfiguration state_interfaces_config; - if (use_all_available_interfaces()) + if (use_urdf_joint_interfaces()) { - state_interfaces_config.type = controller_interface::interface_configuration_type::ALL; + state_interfaces_config.type = + controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT; + for (const auto & joint : model_.joints_) + { + if ( + joint.second->type == urdf::Joint::CONTINUOUS || + joint.second->type == urdf::Joint::REVOLUTE || joint.second->type == urdf::Joint::PRISMATIC) + { + for (const auto & interface : map_interface_to_joint_state_) + { + state_interfaces_config.names.push_back(joint.first + "/" + interface.first); + } + } + } } else { @@ -92,20 +105,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( { params_ = param_listener_->get_params(); - if (params_.publish_dynamic_joint_states) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] The 'publish_dynamic_joint_states' parameter is deprecated and will be removed " - "in future releases. Please update your configuration."); - } - - if (use_all_available_interfaces()) + if (use_urdf_joint_interfaces()) { RCLCPP_INFO( get_node()->get_logger(), - "'joints' or 'interfaces' parameter is empty. " - "All available state interfaces will be published"); + "'joints' or 'interfaces' parameter is empty. Will try to publish all available state " + "interfaces of the URDF joints."); params_.joints.clear(); params_.interfaces.clear(); } @@ -152,16 +157,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( realtime_joint_state_publisher_ = std::make_shared>( joint_state_publisher_); - - if (params_.publish_dynamic_joint_states) - { - dynamic_joint_state_publisher_ = - get_node()->create_publisher( - topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); - realtime_dynamic_joint_state_publisher_ = - std::make_shared>( - dynamic_joint_state_publisher_); - } } catch (const std::exception & e) { @@ -175,10 +170,33 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( is_model_loaded_ = !urdf.empty() && model_.initString(urdf); if (!is_model_loaded_) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", - HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + if (use_urdf_joint_interfaces()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Robot description could not be loaded. Cannot determine URDF joints to publish. " + "Either provide a valid robot description, or explicitly set both 'joints' and " + "'interfaces' parameters."); + return CallbackReturn::ERROR; + } + else + { + if (params_.use_urdf_to_filter) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'use_urdf_to_filter' parameter is set to true, but robot description could not be " + "parsed. Will publish the joints defined in 'joints' parameter without filtering with " + "URDF. Fix the robot description to filter joints with URDF."); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to parse robot description. Will publish the joints defined in 'joints' " + "parameter along with the defined interfaces in 'interface' parameter."); + } + } } // joint_names reserve space for all joints @@ -214,11 +232,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( init_auxiliary_data(); init_joint_state_msg(); - if (params_.publish_dynamic_joint_states) - { - init_dynamic_joint_state_msg(); - } - return CallbackReturn::SUCCESS; } @@ -307,18 +320,16 @@ bool JointStateBroadcaster::init_joint_data() std::reverse(joint_names_.begin(), joint_names_.end()); if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty()) { - std::vector joint_names_filtered; - for (const auto & [joint_name, urdf_joint] : model_.joints_) - { - if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED) - { - if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end()) + // Preserve the order from the first pass; only remove fixed joints + joint_names_.erase( + std::remove_if( + joint_names_.begin(), joint_names_.end(), + [this](const std::string & name) { - joint_names_filtered.push_back(joint_name); - } - } - } - joint_names_ = joint_names_filtered; + const auto urdf_joint = model_.getJoint(name); + return !urdf_joint || urdf_joint->type == urdf::Joint::FIXED; + }), + joint_names_.end()); } // Add extra joints from parameters, each joint will be added to joint_names_ and @@ -395,44 +406,12 @@ void JointStateBroadcaster::init_joint_state_msg() } } -void JointStateBroadcaster::init_dynamic_joint_state_msg() +bool JointStateBroadcaster::use_all_available_interfaces() const { - dynamic_joint_state_msg_.header.frame_id = frame_id_; - dynamic_joint_state_msg_.joint_names.clear(); - dynamic_joint_state_msg_.interface_values.clear(); - for (const auto & name_ifv : name_if_value_mapping_) - { - const auto & name = name_ifv.first; - const auto & interfaces_and_values = name_ifv.second; - dynamic_joint_state_msg_.joint_names.push_back(name); - control_msgs::msg::InterfaceValue if_value; - for (const auto & interface_and_value : interfaces_and_values) - { - if_value.interface_names.emplace_back(interface_and_value.first); - if_value.values.emplace_back(kUninitializedValue); - } - dynamic_joint_state_msg_.interface_values.emplace_back(if_value); - } - - // save dynamic joint state data - dynamic_joint_states_data_.clear(); - for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) - { - dynamic_joint_states_data_.push_back(std::vector()); - - const auto & name = dynamic_joint_state_msg_.joint_names[ji]; - - for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); - ++ii) - { - const auto & interface_name = - dynamic_joint_state_msg_.interface_values[ji].interface_names[ii]; - dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]); - } - } + return this->use_urdf_joint_interfaces(); } -bool JointStateBroadcaster::use_all_available_interfaces() const +bool JointStateBroadcaster::use_urdf_joint_interfaces() const { return params_.joints.empty() || params_.interfaces.empty(); } @@ -463,7 +442,6 @@ controller_interface::return_type JointStateBroadcaster::update( { joint_state_msg_.header.stamp = time; - // update joint state message and dynamic joint state message for (size_t i = 0; i < joint_names_.size(); ++i) { joint_state_msg_.position[i] = joint_states_data_[i].position_; @@ -473,21 +451,6 @@ controller_interface::return_type JointStateBroadcaster::update( realtime_joint_state_publisher_->try_publish(joint_state_msg_); } - if (realtime_dynamic_joint_state_publisher_) - { - dynamic_joint_state_msg_.header.stamp = time; - for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) - { - for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); - ++ii) - { - dynamic_joint_state_msg_.interface_values[ji].values[ii] = - *dynamic_joint_states_data_[ji][ii]; - } - } - realtime_dynamic_joint_state_publisher_->try_publish(dynamic_joint_state_msg_); - } - return controller_interface::return_type::OK; } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index cfe4104ac8..1aba206c6c 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -3,7 +3,7 @@ joint_state_broadcaster: type: bool, default_value: false, read_only: true, - description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." + description: "Defining if ``joint_states`` message should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, @@ -19,7 +19,7 @@ joint_state_broadcaster: type: string_array, default_value: [], read_only: true, - description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0." + description: "Names of extra joints to be added to ``joint_states`` with state set to 0." } interfaces: { type: string_array, @@ -64,9 +64,3 @@ joint_state_broadcaster: read_only: true, description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints." } - publish_dynamic_joint_states: { - type: bool, - default_value: false, - read_only: true, - description: "[Deprecated] Whether to publish dynamic joint states. This parameter is deprecated and will be removed in future releases." - } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 621aed3847..7718dff3c4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -66,7 +66,8 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces, const std::vector & parameter_overrides) { - init_broadcaster_and_set_parameters("", joint_names, interfaces, parameter_overrides); + init_broadcaster_and_set_parameters( + kThreeJointURDF, joint_names, interfaces, parameter_overrides); assign_state_interfaces(joint_names, interfaces); } @@ -182,8 +183,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -215,8 +218,10 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // 3 URDF joints × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -244,8 +249,10 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // params are still cleared from configure (URDF mode), so still INDIVIDUAL_BEST_EFFORT + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -277,8 +284,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -299,30 +308,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) init_broadcaster_and_set_parameters("", JOINT_NAMES, IF_NAMES); assign_state_interfaces(JOINT_NAMES, IF_NAMES); - // configure ok - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - const size_t NUM_JOINTS = joint_names_.size(); - - // check interface configuration - auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); - ASSERT_THAT(cmd_if_conf.names, IsEmpty()); - EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); - auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); - - // publisher initialized - ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); - - // joint state initialized - const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; - ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); - ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + // URDF mode (joints empty) with an unparsable robot description must fail at configure + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription) @@ -349,8 +336,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDes ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // URDF loaded: request specific joints with BEST_EFFORT (3 standard interfaces per joint) + ASSERT_THAT(state_if_conf.names, SizeIs(joint_in_urdf.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -386,8 +375,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // URDF loaded: request specific joints with BEST_EFFORT (3 standard interfaces per joint) + ASSERT_THAT(state_if_conf.names, SizeIs(joint_in_urdf.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -454,8 +445,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); - ASSERT_THAT(state_if_conf.names, IsEmpty()); - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -797,9 +790,25 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) { + // Generate a URDF with all 29 test joints (joint_1 ... joint_29) + std::string perf_urdf = + ""; + for (auto joint = 1u; joint < 30; ++joint) + { + const auto jname = "joint_" + std::to_string(joint); + const auto parent = joint == 1u ? "base_link" : "link_" + std::to_string(joint - 1); + const auto child = "link_" + std::to_string(joint); + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + } + perf_urdf += ""; + controller_interface::ControllerInterfaceParams params; params.controller_name = "joint_state_broadcaster"; - params.robot_description = ""; + params.robot_description = perf_urdf; params.update_rate = 0; params.node_namespace = ""; params.node_options = state_broadcaster_->define_custom_node_options(); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 35980611d5..0dff1769e4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -56,6 +56,40 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest); }; +// Minimal 3-joint URDF covering the joint_names_ used in tests +constexpr const char * kThreeJointURDF = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + class JointStateBroadcasterTest : public ::testing::Test { public: From ee5bab0d8f5ff709445515a4533ec12387e49a2d Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Tue, 19 May 2026 13:32:57 -0400 Subject: [PATCH 26/33] Added velocity limiting to the mecanum controller. (#2313) --- mecanum_drive_controller/CMakeLists.txt | 6 + .../mecanum_drive_controller.hpp | 12 + mecanum_drive_controller/package.xml | 1 + .../src/mecanum_drive_controller.cpp | 138 ++++- .../src/mecanum_drive_controller.yaml | 200 ++++++- .../test/mecanum_drive_controller_params.yaml | 38 ++ .../test/test_mecanum_drive_controller.cpp | 566 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 8 + 8 files changed, 954 insertions(+), 15 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 22593a0701..57529d6ba8 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -8,6 +8,7 @@ export_windows_symbols() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + control_toolbox controller_interface hardware_interface generate_parameter_library @@ -27,8 +28,11 @@ 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.yaml + ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp ) add_library( @@ -42,6 +46,8 @@ target_include_directories(mecanum_drive_controller PUBLIC "$") target_link_libraries(mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters + control_toolbox::rate_limiter_parameters + control_toolbox::control_toolbox controller_interface::controller_interface hardware_interface::hardware_interface pluginlib::pluginlib 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 3a4c148bb5..a638f04fda 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 @@ -26,6 +26,7 @@ #include "control_msgs/msg/mecanum_drive_controller_state.hpp" #include "control_msgs/srv/set_odometry.hpp" +#include "control_toolbox/rate_limiter.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -68,6 +69,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; + void reset_buffers(); + controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -158,6 +161,15 @@ class MecanumDriveController : public controller_interface::ChainableControllerI double velocity_in_center_frame_linear_x_; // [m/s] double velocity_in_center_frame_linear_y_; // [m/s] double velocity_in_center_frame_angular_z_; // [rad/s] + + // Speed limiters + std::unique_ptr> limiter_linear_x_; + std::unique_ptr> limiter_linear_y_; + std::unique_ptr> limiter_angular_z_; + +protected: + // Previous two commands for jerk limiting: queue of [linear_x, linear_y, angular_z] + std::queue> previous_two_commands_; }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 0c3200dc2a..1f47ab85f8 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -27,6 +27,7 @@ backward_ros control_msgs + control_toolbox controller_interface geometry_msgs hardware_interface diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 10d559f779..9f51df50da 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -14,6 +14,7 @@ #include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include #include #include #include @@ -264,6 +265,54 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } + // Configure speed limiters + try + { + 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::invalid_argument & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure linear x speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + try + { + 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::invalid_argument & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure linear y speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + try + { + limiter_angular_z_ = 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::invalid_argument & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure angular z speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Allocate reference interfaces and reset their values to NaN to catch uninitialized usage. + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + reset_buffers(); + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); return controller_interface::CallbackReturn::SUCCESS; @@ -359,11 +408,18 @@ bool MecanumDriveController::on_set_chained_mode(bool /*chained_mode*/) { return controller_interface::CallbackReturn MecanumDriveController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + + // Reset limiter history and reference buffers so a previous activation cannot + // influence the behavior of the controller after a deactivate->activate cycle. + reset_buffers(); + // Try to set default value in command. // If this fails, then another command will be received soon anyways. ControllerReferenceMsg emtpy_msg; reset_controller_reference_msg(emtpy_msg); input_ref_.try_set(emtpy_msg); + + return controller_interface::CallbackReturn::SUCCESS; } @@ -385,9 +441,26 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( return controller_interface::CallbackReturn::FAILURE; } + reset_buffers(); + return controller_interface::CallbackReturn::SUCCESS; } +void MecanumDriveController::reset_buffers() +{ + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + // Replace the queue with one initialized to two zero entries. + previous_two_commands_ = std::queue>( + std::deque>{{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}}); + + // Reset the latest received reference back to NaN so no stale command is applied. + reset_controller_reference_msg(current_ref_); + input_ref_.set(current_ref_); +} + controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { @@ -444,6 +517,34 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ controller_interface::return_type MecanumDriveController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { + if (param_listener_->try_update_params(params_)) + { + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + try + { + limiter_linear_x_->set_params( + 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); + limiter_linear_y_->set_params( + 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); + limiter_angular_z_->set_params( + 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::invalid_argument & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to update speed limiter parameters: %s", e.what()); + } + } + // FORWARD KINEMATICS (odometry). const auto wheel_front_left_state_vel_op = state_interfaces_[FRONT_LEFT].get_optional(); const auto wheel_front_right_state_vel_op = state_interfaces_[FRONT_RIGHT].get_optional(); @@ -497,6 +598,28 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && !std::isnan(reference_interfaces_[2])) { + // Apply speed limits before inverse kinematics + double linear_x_command = reference_interfaces_[0]; + double linear_y_command = reference_interfaces_[1]; + double angular_z_command = reference_interfaces_[2]; + + double & last_linear_x = previous_two_commands_.back()[0]; + double & second_to_last_linear_x = previous_two_commands_.front()[0]; + double & last_linear_y = previous_two_commands_.back()[1]; + double & second_to_last_linear_y = previous_two_commands_.front()[1]; + double & last_angular_z = previous_two_commands_.back()[2]; + double & second_to_last_angular_z = previous_two_commands_.front()[2]; + + limiter_linear_x_->limit( + linear_x_command, last_linear_x, second_to_last_linear_x, period.seconds()); + limiter_linear_y_->limit( + linear_y_command, last_linear_y, second_to_last_linear_y, period.seconds()); + limiter_angular_z_->limit( + angular_z_command, last_angular_z, second_to_last_angular_z, period.seconds()); + + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_x_command, linear_y_command, angular_z_command}}); + tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); /// \note The variables meaning: @@ -507,18 +630,15 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = - rotation_from_base_to_center * - tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + rotation_from_base_to_center * tf2::Vector3(linear_x_command, linear_y_command, 0.0); tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); - velocity_in_center_frame_linear_x_ = - velocity_in_base_frame_w_r_t_center_frame_.x() + - linear_trans_from_base_to_center.y() * reference_interfaces_[2]; - velocity_in_center_frame_linear_y_ = - velocity_in_base_frame_w_r_t_center_frame_.y() - - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; - velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + velocity_in_center_frame_linear_x_ = velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * angular_z_command; + velocity_in_center_frame_linear_y_ = velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * angular_z_command; + velocity_in_center_frame_angular_z_ = angular_z_command; const double wheel_front_left_vel = 1.0 / params_.kinematics.wheels_radius * diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index f789cdb19e..38a2ceac2b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -100,7 +100,7 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Wheel's radius.", - read_only: false, + read_only: true, validation: { gt<>: [0.0] } @@ -110,30 +110,32 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", - read_only: false, + read_only: true, } tf_frame_prefix_enable: { type: bool, default_value: true, description: "Deprecated: this parameter will be removed in a future release. Use 'tf_frame_prefix' instead.", + read_only: true, } tf_frame_prefix: { type: string, default_value: "", description: "(optional) Prefix to be appended to the tf frames, will be added to odom_frame_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + read_only: true, } base_frame_id: { type: string, default_value: "base_link", description: "Base frame_id set to value of base_frame_id.", - read_only: false, + read_only: true, } odom_frame_id: { type: string, default_value: "odom", description: "Odometry frame_id set to value of odom_frame_id.", - read_only: false, + read_only: true, } enable_odom_tf: { type: bool, @@ -146,12 +148,198 @@ mecanum_drive_controller: type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "Diagonal values of twist covariance matrix.", - read_only: false, + read_only: true, } pose_covariance_diagonal: { type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "Diagonal values of pose covariance matrix.", - read_only: false, + read_only: true, } + + linear: + x: + 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] + } + } + 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: + 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] + } + } + 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: + 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] + } + } + 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] + } + } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index dfa3e5de94..d86f6bd211 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -39,3 +39,41 @@ enable_odom_tf: true pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + + test_mecanum_drive_controller_with_limits: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 6.0, 12.0, 18.0, 24.0, 30.0] + + linear: + x: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 + y: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 + angular: + z: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 72c5260a8b..075a14bbe1 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -875,6 +876,571 @@ TEST_F(MecanumDriveControllerTest, odometry_set_service) EXPECT_GT(controller_->odometry_.getY(), start_y); } +// Test that when no velocity limits are configured (all NaN defaults), +// commands pass through immediately without rate limiting. +TEST_F(MecanumDriveControllerTest, test_no_speed_limiter_when_not_configured) +{ + // Use the default config which has no velocity limits set + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + // Send a large step command - without limits it should be applied immediately + controller_->reference_interfaces_[0] = 10.0; + controller_->reference_interfaces_[1] = 5.0; + controller_->reference_interfaces_[2] = 3.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.001)), + controller_interface::return_type::OK); + + // With base_frame_offset = {0,0,0}: + // wheel_fl = 1/r * (vx - vy - k*wz) = 1/0.5 * (10.0 - 5.0 - 1.0*3.0) = 4.0 + // wheel_fr = 1/r * (vx + vy + k*wz) = 1/0.5 * (10.0 + 5.0 + 1.0*3.0) = 36.0 + // wheel_rr = 1/r * (vx - vy + k*wz) = 1/0.5 * (10.0 - 5.0 + 1.0*3.0) = 16.0 + // wheel_rl = 1/r * (vx + vy - k*wz) = 1/0.5 * (10.0 + 5.0 - 1.0*3.0) = 24.0 + const size_t fl = controller_->get_front_left_wheel_index(); + const size_t fr = controller_->get_front_right_wheel_index(); + const size_t rr = controller_->get_rear_right_wheel_index(); + const size_t rl = controller_->get_rear_left_wheel_index(); + + EXPECT_NEAR(4.0, joint_command_values_[fl], 1e-3); + EXPECT_NEAR(36.0, joint_command_values_[fr], 1e-3); + EXPECT_NEAR(16.0, joint_command_values_[rr], 1e-3); + EXPECT_NEAR(24.0, joint_command_values_[rl], 1e-3); +} + +// Test that velocity limits are applied to linear x commands. +// With base_frame_offset = {0,0,0}, wheels_radius = 0.5, and +// sum_of_robot_center_projection_on_X_Y_axis = 1.0: +// For pure linear x velocity v, all 4 wheel velocities = v / wheels_radius = 2*v +TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(0.0, joint_command_values_[0], 1e-3); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + + // Phase 1: Forward acceleration (0 -> 1.0 m/s), max_acceleration = 2.0 m/s² + { + const double linear = 1.0; + const double max_acceleration = 2.0; + const double time_acc = linear / max_acceleration; // 0.5s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + // Fill queue at steady state + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 2: Forward deceleration (1.0 -> 0.0 m/s), max_deceleration = -4.0 m/s² + { + const double linear = 0.0; + const double max_deceleration = -4.0; + const double time_acc = -1.0 / max_deceleration; // 0.25s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 3: Reverse acceleration (0 -> -1.0 m/s), max_acceleration_reverse = -8.0 m/s² + { + const double linear = -1.0; + const double max_acceleration_reverse = -8.0; + const double time_acc = -1.0 / max_acceleration_reverse; // 0.125s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 4: Reverse deceleration (-1.0 -> 0.0 m/s), max_deceleration_reverse = 10.0 m/s² + { + const double linear = 0.0; + const double max_deceleration_reverse = 10.0; + const double time_acc = 1.0 / max_deceleration_reverse; // 0.1s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } +} + +// Test that velocity limits are applied to linear y commands. +// For pure linear y velocity v with zero base_frame_offset: +// wheel_fl = -v/r, wheel_fr = v/r, wheel_rr = -v/r, wheel_rl = v/r +TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_y) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double linear_y = 1.0; + const double max_acceleration = 2.0; + const double time_acc = linear_y / max_acceleration; // 0.5s + + // Accelerate in linear y from 0 to 1.0 m/s + // wheel_fr = linear_y / wheels_radius (positive) + const size_t fr = controller_->get_front_right_wheel_index(); + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = linear_y; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear_y / wheels_radius, joint_command_values_[fr]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = linear_y; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear_y / wheels_radius, joint_command_values_[fr], 1e-3); +} + +// Test that velocity limits are applied to angular z commands. +// For pure angular z velocity w with zero base_frame_offset: +// wheel_fr = k*w/r (positive), wheel_fl = -k*w/r (negative) +// where k = sum_of_robot_center_projection_on_X_Y_axis = 1.0 +TEST_F(MecanumDriveControllerTest, test_speed_limiter_angular_z) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double k = 1.0; // sum_of_robot_center_projection_on_X_Y_axis + const double angular_z = 1.0; + const double max_acceleration = 2.0; + const double time_acc = angular_z / max_acceleration; // 0.5s + + // Accelerate in angular z from 0 to 1.0 rad/s + // wheel_fr = k * angular_z / wheels_radius (positive) + const size_t fr = controller_->get_front_right_wheel_index(); + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = angular_z; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(k * angular_z / wheels_radius, joint_command_values_[fr]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = angular_z; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(k * angular_z / wheels_radius, joint_command_values_[fr], 1e-3); +} + +// Test that reset_buffers() clears the jerk-limiter history, the reference interfaces, +// and the latest received reference back to NaN/zero. +TEST_F(MecanumDriveControllerTest, test_reset_buffers_clears_limiter_state) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Dirty all buffers that reset_buffers() is responsible for clearing. + controller_->reference_interfaces_[0] = 1.0; + controller_->reference_interfaces_[1] = 2.0; + controller_->reference_interfaces_[2] = 3.0; + + std::queue> dirty; + dirty.push({{4.0, 5.0, 6.0}}); + dirty.push({{7.0, 8.0, 9.0}}); + std::swap(controller_->previous_two_commands_, dirty); + + ControllerReferenceMsg dirty_ref; + dirty_ref.header.stamp = controller_->get_node()->now(); + dirty_ref.twist.linear.x = 1.0; + dirty_ref.twist.linear.y = 2.0; + dirty_ref.twist.angular.z = 3.0; + controller_->input_ref_.set(dirty_ref); + + controller_->reset_buffers(); + + for (const auto & itf : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(itf)); + } + ASSERT_EQ(controller_->previous_two_commands_.size(), 2u); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + auto reset_ref = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reset_ref.twist.linear.x)); + EXPECT_TRUE(std::isnan(reset_ref.twist.linear.y)); + EXPECT_TRUE(std::isnan(reset_ref.twist.angular.z)); +} + +// Test that lifecycle transitions reset the limiter history so that re-activating +// the controller re-limits commands from zero. +TEST_F(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buffers) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double linear = 1.0; + const double max_acceleration = 2.0; // m/s^2 (from test_mecanum_drive_controller_params.yaml) + const double time_acc = linear / max_acceleration; + + // Ramp up linear x to the steady-state target so the limiter buffer holds + // non-zero history. + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) + 5; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + EXPECT_NEAR(linear, controller_->previous_two_commands_.back()[0], 1e-3); + + // Deactivate then re-activate: limiter history must be reset to zero. + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + // After reactivation, requesting the same target should once again be limited + // by max_acceleration starting from zero, not pass through immediately. + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(joint_command_values_[0], linear / wheels_radius) + << "Limiter history was not reset across lifecycle transitions; the wheel command " + "should be ramping up from zero again."; +} + +// This test verifies that parameters can be updated at runtime. +TEST_F(MecanumDriveControllerTest, test_speed_limiter_runtime_update) +{ + // If you set a linear velocity reference without acceleration limits, + // then the wheel velocity command (rotations/s) will be: + // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m). + // (The velocity command looks like a step function). + // However, if there are acceleration limits, then the actual wheel velocity command + // should always be less than the ideal velocity, and should only become + // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2). + const double max_acceleration_1 = 2.0; + const double max_acceleration_2 = 5.0; + const double max_deceleration = -4.0; + + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + const double dt = 0.001; + const double wheels_radius = 0.5; + + auto wait_for_limiter = [&](double linear_ref, double expected_vel) + { + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear_ref; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(expected_vel, joint_command_values_[0], 1e-3); + } + }; + + // wait for the speed limiter to fill the queue + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + wait_for_limiter(0.0, 0.0); + + // Phase 1: accelerate with max_acceleration = 2.0 + { + const double linear = 1.0; + const double time_acc = linear / max_acceleration_1; + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, linear / wheels_radius); + } + // Stop the robot + { + const double linear = 0.0; + const double time_dec = 1.0 / std::abs(max_deceleration); + for (int i = 0; i < static_cast(std::floor(time_dec / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, 0.0); + } + // Phase 2: update parameter at runtime to max_acceleration = 5.0 + { + auto result = controller_->get_node()->set_parameter( + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_2))); + ASSERT_TRUE(result.successful); + } + // Phase 3: accelerate with max_acceleration = 5.0 + { + const double linear = 1.0; + const double time_acc_1 = linear / max_acceleration_1; + const double time_acc_2 = linear / max_acceleration_2; + // With higher acceleration, should reach target faster + ASSERT_LT(time_acc_2, time_acc_1); + for (int i = 0; i < static_cast(std::floor(time_acc_2 / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, linear / wheels_radius); + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index d625c302b4..7a9396b31e 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -93,6 +93,14 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_prefix_set_namespace); FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_tilde_prefix_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, test_no_speed_limiter_when_not_configured); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_x); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_y); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_angular_z); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_runtime_update); + FRIEND_TEST(MecanumDriveControllerTest, test_reset_buffers_clears_limiter_state); + FRIEND_TEST(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buffers); + public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override From d5ec4be23b2129075a610f7117f52c09dd697ce1 Mon Sep 17 00:00:00 2001 From: Dhruvil Parikh <41384593+dparikh79@users.noreply.github.com> Date: Thu, 21 May 2026 06:16:23 -0400 Subject: [PATCH 27/33] [JTC] Fix segfault when last trajectory segment is skipped (#2359) --- .../src/trajectory.cpp | 27 ++++++ .../test/test_trajectory.cpp | 95 +++++++++++++++++++ 2 files changed, 122 insertions(+) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index d285ede823..b23dc52952 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -209,7 +209,34 @@ bool Trajectory::sample( start_segment_itr = --end(); end_segment_itr = end(); last_sample_idx_ = last_idx; + + // If the last segment was never entered (e.g. sample_time jumped past a very + // short last segment because the controller rate is slower than the segment + // duration), the last trajectory point's positions may have never been + // deduced from velocities. Recover by deducing them from the previous point + // if that point has positions; otherwise return false so the caller does not + // dereference an empty positions vector downstream. See #2282. + if (trajectory_msg_->points[last_idx].positions.empty() && last_idx > 0) + { + auto & prev_point = trajectory_msg_->points[last_idx - 1]; + auto & last_point = trajectory_msg_->points[last_idx]; + if (!prev_point.positions.empty()) + { + const rclcpp::Time t_prev = trajectory_start_time_ + prev_point.time_from_start; + const rclcpp::Time t_last = trajectory_start_time_ + last_point.time_from_start; + deduce_from_derivatives( + prev_point, last_point, state_before_traj_msg_.positions.size(), + (t_last - t_prev).seconds()); + } + } + output_state = (*start_segment_itr); + if (output_state.positions.empty()) + { + start_segment_itr = end(); + end_segment_itr = end(); + return false; + } // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) { diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 06e508a12b..af6a7939f2 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -1046,3 +1046,98 @@ TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset) EXPECT_EQ(current_position[1], next_position[1]); EXPECT_EQ(current_position[2], next_position[2]); } +// Regression test for #2282: with a velocity-only trajectory whose last +// segment is skipped (sample_time advances past the last segment without +// entering it, e.g. controller rate slower than the last segment duration), +// the last point's positions never get deduced from velocities. Before the +// fix, the "whole animation has played out" branch would copy that point +// into output_state with empty positions, which then segfaults later in +// JointTrajectoryController::compute_error_for_joint. +TEST(TestTrajectory, sample_velocity_only_skipped_last_segment_deduces_positions) +{ + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + p1.positions = {0.0}; + p1.velocities = {1.0}; + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + // intentionally velocity-only; positions will be deduced + p2.velocities = {1.0}; + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.time_from_start = rclcpp::Duration::from_seconds(2.001); + // intentionally velocity-only and tiny duration to model the skipped-last-segment case + p3.velocities = {1.0}; + + auto traj_msg = std::make_shared(); + traj_msg->points = {p1, p2, p3}; + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, p1, traj_msg); + + trajectory_msgs::msg::JointTrajectoryPoint output_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // First sample at time_now anchors trajectory_start_time_ = time_now (header + // stamp left default so the sample() code path that sets it on first call + // fires). Second sample inside segment 0 so p2's positions get deduced from + // velocities. + traj.sample(time_now, DEFAULT_INTERPOLATION, output_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, output_state, start, + end); + ASSERT_FALSE(output_state.positions.empty()); + + // Now sample past the end: segment 1 (between p2 and p3, 1 ms) is skipped + // entirely, leaving p3.positions empty without the fix. + const bool ok = traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, output_state, start, + end); + + EXPECT_TRUE(ok); + ASSERT_FALSE(output_state.positions.empty()) + << "sample() must not return success with empty positions; would segfault " + "in JointTrajectoryController::compute_error_for_joint"; + EXPECT_EQ(output_state.positions.size(), p1.positions.size()); +} +// Regression test for #2282 pathological case: every point in the trajectory +// is velocity-only and sample_time is so far past the end that no segment +// was ever entered to deduce positions. With no usable position chain, the +// "whole animation has played out" branch must return false rather than +// copy empty positions into output_state. +TEST(TestTrajectory, sample_velocity_only_all_segments_skipped_returns_false) +{ + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.time_from_start = rclcpp::Duration::from_seconds(0.001); + p1.velocities = {1.0}; + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.time_from_start = rclcpp::Duration::from_seconds(0.002); + p2.velocities = {1.0}; + + auto traj_msg = std::make_shared(); + traj_msg->points = {p1, p2}; + + trajectory_msgs::msg::JointTrajectoryPoint state_before; + // intentionally empty positions on the prior-state too, to force the + // "no usable position anywhere" case + state_before.velocities = {1.0}; + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, state_before, traj_msg); + + trajectory_msgs::msg::JointTrajectoryPoint output_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Anchor trajectory_start_time_ = time_now via a first sample. With + // state_before.positions also empty, no segment will ever produce non-empty + // positions, so the "whole animation has played out" branch must return false. + traj.sample(time_now, DEFAULT_INTERPOLATION, output_state, start, end); + const bool ok = traj.sample( + time_now + rclcpp::Duration::from_seconds(10.0), DEFAULT_INTERPOLATION, output_state, start, + end); + + EXPECT_FALSE(ok) + << "sample() must return false when it cannot produce a non-empty positions vector"; +} From aa4079f13b4bda0198d5db79467aad72619034b4 Mon Sep 17 00:00:00 2001 From: Souri Rishik Date: Thu, 21 May 2026 16:45:21 +0530 Subject: [PATCH 28/33] Fix motion_primitive_controller TOC in docs (#2221) --- .pre-commit-config.yaml | 2 +- doc/controllers_index.rst | 6 ++- .../{ => doc}/README.md | 53 +++++++++---------- motion_primitives_controllers/doc/userdoc.rst | 26 +++++++++ motion_primitives_controllers/userdoc.rst | 19 ------- 5 files changed, 55 insertions(+), 51 deletions(-) rename motion_primitives_controllers/{ => doc}/README.md (65%) create mode 100644 motion_primitives_controllers/doc/userdoc.rst delete mode 100644 motion_primitives_controllers/userdoc.rst diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9ee9a07bd1..126c34d37b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -116,7 +116,7 @@ repos: args: [ '--max-line-length=100', '--ignore=D001', - '--ignore-path=motion_primitives_controllers/userdoc.rst' # D000 fails with myst_parser + '--ignore-path=motion_primitives_controllers/doc/userdoc.rst' # D000 fails with myst_parser ] exclude: CHANGELOG\.rst$ diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 70ebc1769d..f566def736 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -41,11 +41,13 @@ Controllers for Manipulators and Other Robots Admittance Controller <../admittance_controller/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + Motion Primitive Controller <../motion_primitives_controllers/doc/userdoc.rst> Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> - Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> - Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst> + Position Controllers <../position_controllers/doc/userdoc.rst> + Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Broadcasters ********************** diff --git a/motion_primitives_controllers/README.md b/motion_primitives_controllers/doc/README.md similarity index 65% rename from motion_primitives_controllers/README.md rename to motion_primitives_controllers/doc/README.md index fc2b76b4fe..a1b7a7e560 100644 --- a/motion_primitives_controllers/README.md +++ b/motion_primitives_controllers/doc/README.md @@ -1,26 +1,5 @@ -motion_primitive_controllers -========================================== - -Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC) - -# Description -This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented. - -- Supported motion primitives: - - `LINEAR_JOINT` - - `LINEAR_CARTESIAN` - - `CIRCULAR_CARTESIAN` - -If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. - -The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent. - -![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png) - -This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/examples.py) in the `Universal_Robots_ROS2_Driver` package. - ## Command and State Interfaces -To transmit the motion primitives, the following command and state interfaces are required. All interfaces use the naming scheme `tf_prefix_ + "motion_primitive/"` where the `tf_prefix` is provided to the controller as a parameter. +The controller uses the following command and state interfaces to transmit the motion primitives. All interfaces use the naming scheme `tf_prefix_ + "motion_primitive/"` where the `tf_prefix` is provided to the controller as a parameter. ### Command Interfaces These interfaces are used to send motion primitive data to the hardware interface: @@ -46,17 +25,33 @@ These interfaces are used to communicate the internal status of the hardware int - `STOPPED`: The robot was stopped using the `STOP_MOTION` command and must be reset with the `RESET_STOP` command before executing new commands. - `ready_for_new_primitive`: Boolean flag indicating whether the interface is ready to receive a new motion primitive -# Architecture Overview +## motion_primitives_forward_controller +This controller provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented. + +- Supported motion primitives: + - `LINEAR_JOINT` + - `LINEAR_CARTESIAN` + - `CIRCULAR_CARTESIAN` + +If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. + +The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent. + +![Action Image](Moprim_Controller_ExecuteMotion_Action.drawio.png) + +This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py) in the `Universal_Robots_ROS2_Driver` package. + +### Architecture overview of motion_primitives_forward_controller Architecture for a UR robot with [`Universal_Robots_ROS2_Driver` in motion primitives mode](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver). -![UR Robot Architecture](doc/ros2_control_motion_primitives_ur.drawio.png) +![UR Robot Architecture](ros2_control_motion_primitives_ur.drawio.png) Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver/kuka_eki_motion_primitives_hw_interface). -![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png) +![KUKA Robot Architecture](ros2_control_motion_primitives_kuka.drawio.png) -# Demo-Video with UR10e -[![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ) +### Demo-Video with UR10e and motion_primitives_forward_controller +[![UR demo video](moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ) -# Demo-Video with KR3 -[![KUKA demo video](doc/moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg) +### Demo-Video with KR3 and motion_primitives_forward_controller +[![KUKA demo video](moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg) diff --git a/motion_primitives_controllers/doc/userdoc.rst b/motion_primitives_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..2073955deb --- /dev/null +++ b/motion_primitives_controllers/doc/userdoc.rst @@ -0,0 +1,26 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/motion_primitives_controllers/doc/userdoc.rst + +.. _motion_primitives_controllers_userdoc: + +motion_primitive_controllers +========================================== + +Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC) + +.. warning:: + There is no guarantee that the motion defined by the motion primitive will actually be executed exactly as planned. In particular, for motions in Cartesian space such as LIN primitives, it is not necessarily ensured that the robot will execute the motion exactly in that way, since the inverse kinematics is not always unique. + +.. include:: README.md + :parser: myst_parser.sphinx_ + +Parameters +---------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `__ contains descriptions for all the parameters used by this controller. + +.. generate_parameter_library_details:: ../src/motion_primitives_forward_controller_parameter.yaml + +An example parameter file for this controller can be found in `the test directory `__: + +.. literalinclude:: ../test/motion_primitives_forward_controller_params.yaml + :language: yaml diff --git a/motion_primitives_controllers/userdoc.rst b/motion_primitives_controllers/userdoc.rst deleted file mode 100644 index e199afa449..0000000000 --- a/motion_primitives_controllers/userdoc.rst +++ /dev/null @@ -1,19 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/motion_primitives_controllers/userdoc.rst - -.. _motion_primitives_controllers_userdoc: - - -.. include:: README.md - :parser: myst_parser.sphinx_ - -Parameters -,,,,,,,,,,, - -This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. - -.. generate_parameter_library_details:: src/motion_primitives_forward_controller_parameter.yaml - -An example parameter file for this controller can be found in `the test directory `_: - -.. literalinclude:: test/motion_primitives_forward_controller_params.yaml - :language: yaml From b074eb41b14351df5ce32b2009437e180ce31f60 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 21 May 2026 13:30:25 +0100 Subject: [PATCH 29/33] Remove broken toctree entries for deleted position/velocity controllers (#2368) --- doc/controllers_index.rst | 2 -- 1 file changed, 2 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index f566def736..b4c08babc9 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -46,8 +46,6 @@ Controllers for Manipulators and Other Robots Motion Primitive Controller <../motion_primitives_controllers/doc/userdoc.rst> Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> - Position Controllers <../position_controllers/doc/userdoc.rst> - Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Broadcasters ********************** From ad79f26235087b29ede3298a36777dae803a5e61 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 26 May 2026 14:32:54 +0200 Subject: [PATCH 30/33] Add broadcaster for magnetic field values from a magnetometer (#2214) --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + magnetometer_broadcaster/CMakeLists.txt | 81 +++++++ magnetometer_broadcaster/doc/userdoc.rst | 24 ++ .../magnetometer_broadcaster.xml | 10 + magnetometer_broadcaster/package.xml | 45 ++++ .../src/magnetometer_broadcaster.cpp | 147 ++++++++++++ .../magnetometer_broadcaster_parameters.yaml | 29 +++ .../test/magnetometer_broadcaster_params.yaml | 4 + .../test_load_magnetometer_broadcaster.cpp | 46 ++++ .../test/test_magnetometer_broadcaster.cpp | 209 ++++++++++++++++++ ros2_controllers/package.xml | 1 + 12 files changed, 601 insertions(+) create mode 100644 magnetometer_broadcaster/CMakeLists.txt create mode 100644 magnetometer_broadcaster/doc/userdoc.rst create mode 100644 magnetometer_broadcaster/magnetometer_broadcaster.xml create mode 100644 magnetometer_broadcaster/package.xml create mode 100644 magnetometer_broadcaster/src/magnetometer_broadcaster.cpp create mode 100644 magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml create mode 100644 magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml create mode 100644 magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp create mode 100644 magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index b4c08babc9..ecaf5a3664 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -63,6 +63,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst> State Interfaces Broadcaster <../state_interfaces_broadcaster/doc/userdoc.rst> + Magnetometer Broadcaster <../magnetometer_broadcaster/doc/userdoc.rst> Filters ********************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index df3986cd6b..8a6174108e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -57,3 +57,7 @@ steering_controllers_library ***************************** * Parameter ``tf_frame_prefix`` added with the similar functionality to other controllers. (`#2080 `_). * Set odometry service added to be used at runtime. (`#2244 `_). + +magnetometer_broadcaster +************************ +New package to broadcast ``sensor_msgs/msg/MagneticField`` from state interfaces defined by the ``semantic_components::MagneticFieldSensor``. diff --git a/magnetometer_broadcaster/CMakeLists.txt b/magnetometer_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..943a34ca9d --- /dev/null +++ b/magnetometer_broadcaster/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.16) +project(magnetometer_broadcaster) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(magnetometer_broadcaster_parameters + src/magnetometer_broadcaster_parameters.yaml +) + +add_library(magnetometer_broadcaster SHARED + src/magnetometer_broadcaster.cpp +) +target_compile_features(magnetometer_broadcaster PUBLIC cxx_std_20) +target_link_libraries(magnetometer_broadcaster PUBLIC + magnetometer_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS}) + +pluginlib_export_plugin_description_file( + controller_interface magnetometer_broadcaster.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_magnetometer_broadcaster test/test_load_magnetometer_broadcaster.cpp) + target_link_libraries(test_load_magnetometer_broadcaster + magnetometer_broadcaster + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_magnetometer_broadcaster + test/test_magnetometer_broadcaster.cpp + ) + target_link_libraries(test_magnetometer_broadcaster + magnetometer_broadcaster + ros2_control_test_assets::ros2_control_test_assets + ) +endif() + +install( + TARGETS + magnetometer_broadcaster + magnetometer_broadcaster_parameters + EXPORT export_magnetometer_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_magnetometer_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/magnetometer_broadcaster/doc/userdoc.rst b/magnetometer_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..7568d7b0bd --- /dev/null +++ b/magnetometer_broadcaster/doc/userdoc.rst @@ -0,0 +1,24 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/magnetometer_broadcaster/doc/userdoc.rst + +.. _magnetometer_broadcaster_userdoc: + +Magnetometer Broadcaster +-------------------------------- +Broadcaster for magnetometer messages (type: ``sensor_msgs/msg/MagneticField``), using the ``semantic_components::MagneticFieldSensor``. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/magnetometer_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/magnetometer_broadcaster_params.yaml + :language: yaml diff --git a/magnetometer_broadcaster/magnetometer_broadcaster.xml b/magnetometer_broadcaster/magnetometer_broadcaster.xml new file mode 100644 index 0000000000..0e2214e395 --- /dev/null +++ b/magnetometer_broadcaster/magnetometer_broadcaster.xml @@ -0,0 +1,10 @@ + + + + This controller publishes the readings of a magnetometer as sensor_msgs/msg/MagneticField message. + + + diff --git a/magnetometer_broadcaster/package.xml b/magnetometer_broadcaster/package.xml new file mode 100644 index 0000000000..ffbee87b30 --- /dev/null +++ b/magnetometer_broadcaster/package.xml @@ -0,0 +1,45 @@ + + + + magnetometer_broadcaster + 6.7.0 + Controller to publish readings of a magnetometer. + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Christian Rauch + + ament_cmake + + ros2_control_cmake + + backward_ros + controller_interface + eigen + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/magnetometer_broadcaster/src/magnetometer_broadcaster.cpp b/magnetometer_broadcaster/src/magnetometer_broadcaster.cpp new file mode 100644 index 0000000000..980eb7b55d --- /dev/null +++ b/magnetometer_broadcaster/src/magnetometer_broadcaster.cpp @@ -0,0 +1,147 @@ +// Copyright 2026 Christian Rauch +// Copyright 2021 PAL Robotics SL. +// +// 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. + +/* + * Authors: Christian Rauch, Subhas Das, Denis Stogl, Victor Lopez + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_broadcaster +{ + +class MagnetometerBroadcaster : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return {}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return { + .type = controller_interface::interface_configuration_type::INDIVIDUAL, + .names = magnetometer_->get_state_interface_names(), + }; + } + + controller_interface::CallbackReturn on_init() override + { + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Exception thrown during init stage with message: " << e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) override + { + try + { + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Exception thrown during config stage with message: " << e.what()); + return CallbackReturn::ERROR; + } + + magnetometer_ = std::make_unique(params_.sensor_name); + try + { + sensor_state_publisher_ = get_node()->create_publisher( + "~/magnetic_field", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Exception thrown during publisher creation at configure stage with message: " << e.what()); + return CallbackReturn::ERROR; + } + + state_message_.header.frame_id = params_.frame_id; + std::copy( + params_.static_covariance.begin(), params_.static_covariance.end(), + state_message_.magnetic_field_covariance.begin()); + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) override + { + magnetometer_->assign_loaned_state_interfaces(state_interfaces_); + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) override + { + magnetometer_->release_interfaces(); + return CallbackReturn::SUCCESS; + } + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) override + { + magnetometer_->get_values_as_message(state_message_); + + if (realtime_publisher_) + { + state_message_.header.stamp = time; + realtime_publisher_->try_publish(state_message_); + } + + return controller_interface::return_type::OK; + } + +protected: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr magnetometer_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; + sensor_msgs::msg::MagneticField state_message_; +}; + +} // namespace magnetometer_broadcaster + +PLUGINLIB_EXPORT_CLASS( + magnetometer_broadcaster::MagnetometerBroadcaster, controller_interface::ControllerInterface) diff --git a/magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml b/magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml new file mode 100644 index 0000000000..d52e3c1dc9 --- /dev/null +++ b/magnetometer_broadcaster/src/magnetometer_broadcaster_parameters.yaml @@ -0,0 +1,29 @@ +magnetometer_broadcaster: + sensor_name: { + type: string, + default_value: "", + read_only: true, + description: "Defines sensor name used as prefix for its interfaces. + Interface names are: ``/magnetic_field.x, /magnetic_field.x, /magnetic_field.z.``", + validation: { + not_empty<>: null + } + } + frame_id: { + type: string, + default_value: "", + read_only: true, + description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } + } + static_covariance: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + read_only: true, + description: "Static covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } diff --git a/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml b/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml new file mode 100644 index 0000000000..6efe0a9b5c --- /dev/null +++ b/magnetometer_broadcaster/test/magnetometer_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_magnetometer_broadcaster: + ros__parameters: + sensor_name: magnetometer + frame_id: magnetometer_frame diff --git a/magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp b/magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp new file mode 100644 index 0000000000..087c39d7f5 --- /dev/null +++ b/magnetometer_broadcaster/test/test_load_magnetometer_broadcaster.cpp @@ -0,0 +1,46 @@ +// Copyright 2026 ros2_control development team +// +// 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. + +/* + * Authors: Christian Rauch, Wiktor Bajor, Jakub Delicat + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +TEST(TestLoadMagnetometerBroadcaster, load_controller) +{ + rclcpp::init(0, nullptr); + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/magnetometer_broadcaster_params.yaml"; + + cm.set_parameter({"test_magnetometer_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_magnetometer_broadcaster.type", "magnetometer_broadcaster/MagnetometerBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_magnetometer_broadcaster"), nullptr); + rclcpp::shutdown(); +} diff --git a/magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp b/magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp new file mode 100644 index 0000000000..bf8160ee33 --- /dev/null +++ b/magnetometer_broadcaster/test/test_magnetometer_broadcaster.cpp @@ -0,0 +1,209 @@ +// Copyright 2026 ros2_control development team +// +// 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. + +/* + * Authors: Christian Rauch, Wiktor Bajor, Jakub Delicat + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using callback_return_type = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace +{ +rclcpp::WaitResultKind wait_for(std::shared_ptr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + return wait_set.wait().kind(); +} + +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class MagnetometerBroadcasterTest : public ::testing::Test +{ +public: + void SetUp() + { + broadcaster = loader->createInstance( + "magnetometer_broadcaster::MagnetometerBroadcaster"); + } + + void TearDown() { broadcaster.reset(); } + + void setup_broadcaster() + { + std::vector state_ifs; + state_ifs.emplace_back(magnetic_field_x, nullptr); + state_ifs.emplace_back(magnetic_field_y, nullptr); + state_ifs.emplace_back(magnetic_field_z, nullptr); + + broadcaster->assign_interfaces({}, std::move(state_ifs)); + } + + sensor_msgs::msg::MagneticField subscribe_and_get_message() + { + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subscription = test_subscription_node.create_subscription( + "/test_magnetometer_broadcaster/magnetic_field", 10, + [](const sensor_msgs::msg::MagneticField::SharedPtr) {}); + broadcaster->update(rclcpp::Time{}, rclcpp::Duration::from_seconds(0)); + wait_for(subscription); + + rclcpp::MessageInfo msg_info; + sensor_msgs::msg::MagneticField msg; + subscription->take(msg, msg_info); + return msg; + } + + controller_interface::ControllerInterfaceParams create_ctrl_params( + const rclcpp::NodeOptions & node_options, const std::string & robot_description = "") + { + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_magnetometer_broadcaster"; + params.robot_description = robot_description; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + return params; + } + +protected: + const std::string sensor_name_ = "magnetometer"; + const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", sensor_name_); + const std::string frame_id_ = "magnetometer_frame"; + const rclcpp::Parameter frame_id_param_ = rclcpp::Parameter("frame_id", frame_id_); + + std::array sensor_values_ = {{20e-6, 30e-6, 40e-6}}; + hardware_interface::StateInterface::SharedPtr magnetic_field_x = + std::make_shared( + sensor_name_, "magnetic_field.x", &sensor_values_[0]); + hardware_interface::StateInterface::SharedPtr magnetic_field_y = + std::make_shared( + sensor_name_, "magnetic_field.y", &sensor_values_[1]); + hardware_interface::StateInterface::SharedPtr magnetic_field_z = + std::make_shared( + sensor_name_, "magnetic_field.z", &sensor_values_[2]); + + std::unique_ptr loader = + std::make_unique(std::string{}); + + controller_interface::ControllerInterfaceSharedPtr broadcaster = nullptr; +}; + +TEST_F(MagnetometerBroadcasterTest, whenNoParamsAreSetThenInitShouldFail) +{ + const auto result = broadcaster->init(create_ctrl_params( + broadcaster->define_custom_node_options(), ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(MagnetometerBroadcasterTest, whenOnlySensorNameIsSetThenInitShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_}); + const auto result = broadcaster->init( + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F( + MagnetometerBroadcasterTest, + whenAllRequiredArgumentsAreSetThenInitConfigureAndActivationShouldSucceed) +{ + const auto node_options = + create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_param_}); + const auto result = broadcaster->init( + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); +} + +TEST_F(MagnetometerBroadcasterTest, whenBroadcasterIsActiveShouldPublishWithCovarianceSetToZero) +{ + const auto node_options = + create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_param_}); + const auto result = broadcaster->init( + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + setup_broadcaster(); + ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + const auto msg = subscribe_and_get_message(); + EXPECT_EQ(msg.header.frame_id, frame_id_); + EXPECT_EQ(msg.magnetic_field.x, sensor_values_[0]); + EXPECT_EQ(msg.magnetic_field.y, sensor_values_[1]); + EXPECT_EQ(msg.magnetic_field.z, sensor_values_[2]); + + const std::array expected_covariance = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + ASSERT_THAT(msg.magnetic_field_covariance, ::testing::ElementsAreArray(expected_covariance)); +} + +TEST_F( + MagnetometerBroadcasterTest, + whenBroadcasterIsActiveAndStaticCovarianceShouldPublishWithStaticCovariance) +{ + const std::array static_covariance = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}}; + const auto node_options = create_node_options_with_overriden_parameters( + {sensor_name_param_, + frame_id_param_, + {"static_covariance", + std::vector{static_covariance.begin(), static_covariance.end()}}}); + const auto result = broadcaster->init( + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(broadcaster->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + setup_broadcaster(); + ASSERT_EQ(broadcaster->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + const auto msg = subscribe_and_get_message(); + EXPECT_EQ(msg.header.frame_id, frame_id_); + EXPECT_EQ(msg.magnetic_field.x, sensor_values_[0]); + EXPECT_EQ(msg.magnetic_field.y, sensor_values_[1]); + EXPECT_EQ(msg.magnetic_field.z, sensor_values_[2]); + + ASSERT_THAT(msg.magnetic_field_covariance, ::testing::ElementsAreArray(static_covariance)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 77f1264208..a05f9e73fb 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -29,6 +29,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + magnetometer_broadcaster mecanum_drive_controller motion_primitives_controllers omni_wheel_drive_controller From e89c34201b1ca92e1ce9f3ed8dcfb3578a98e238 Mon Sep 17 00:00:00 2001 From: Shivam Maurya Date: Tue, 26 May 2026 18:04:40 +0530 Subject: [PATCH 31/33] Fix SPDX license expression (#2370) --- ros2_controllers_test_nodes/setup.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 9be847b0b4..9bd0823fa1 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -36,14 +36,13 @@ keywords=["ROS"], classifiers=[ "Intended Audience :: Developers", - "License :: OSI Approved :: Apache Software License", "Programming Language :: Python", "Topic :: Software Development", ], description="Demo nodes for showing and testing functionalities of ros2_control framework.", long_description="""\ Demo nodes for showing and testing functionalities of the ros2_control framework.""", - license="Apache License, Version 2.0", + license="Apache-2.0", extras_require={ "test": [ "pytest", From fb0b8e0edb30ce38aa6a4090b16723f8d88bf58e Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Wed, 27 May 2026 03:16:49 +0530 Subject: [PATCH 32/33] fix: resolve merge conflicts Signed-off-by: Ishan1923 --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index e916fd4246..79ee7e68b3 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -412,16 +412,12 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate( // influence the behavior of the controller after a deactivate->activate cycle. reset_buffers(); -<<<<<<< HEAD // Try to set default value in command. // If this fails, then another command will be received soon anyways. ControllerReferenceMsg emtpy_msg; reset_controller_reference_msg(emtpy_msg); input_ref_.try_set(emtpy_msg); - -======= ->>>>>>> origin/master return controller_interface::CallbackReturn::SUCCESS; } From e29f848480bf7ccc4d886c843e920a0796a8cd10 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Wed, 27 May 2026 14:13:09 +0530 Subject: [PATCH 33/33] fix: pass correct arguments for move_robot() Signed-off-by: Ishan1923 --- .../test/test_steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index d597262d68..7ef6a62961 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -425,7 +425,7 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0); ASSERT_GT(controller_->odometry_.get_x(), 0.0); - move_robot(0.0, 0.0, 0.0); + move_robot(0.0, 0.0); // 2. Call the odometry set service auto set_request = std::make_shared();