From 615524831a3bd3e0fc4e4678c6eb9afd7f39f326 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jun 2024 10:21:17 +0200 Subject: [PATCH 01/27] [Steering controllers library] Reference interfaces are body twist (#1168) --- .../test/test_ackermann_steering_controller.cpp | 2 +- .../test/test_ackermann_steering_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- steering_controllers_library/doc/userdoc.rst | 4 +++- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 4 ++-- .../test/test_steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 13 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..38cca0cac9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -84,7 +84,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49331ae8a2..363db793d5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -302,7 +302,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..fd8565853f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -68,7 +68,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 390447c25f..dcdb7ed169 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -266,7 +266,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index a52e34c120..44b180162e 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -56,7 +56,9 @@ References (from a preceding controller) Used when controller is in chained mode (``in_chained_mode == true``). - ``/linear/velocity`` double, in m/s -- ``/angular/position`` double, in rad +- ``/angular/velocity`` double, in rad/s + +representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c11d90dc6f..e417b9fcd1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -131,7 +131,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // name constants for reference interfaces size_t nr_ref_itfs_; - // store last velocity + // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5880000d17..5445196a6e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -324,7 +324,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; @@ -396,7 +396,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store and set commands + // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..98ab97fdc3 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -66,7 +66,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 05ac17b454..b6fe1770c1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -324,7 +324,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 1807ab59a8..b4d58a95c0 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -285,7 +285,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From b81be48fb1b6101a3c601c0ea11c5393e5a7026e Mon Sep 17 00:00:00 2001 From: Enrique Llorente Pastora Date: Wed, 19 Jun 2024 19:31:22 +0200 Subject: [PATCH 02/27] [STEERING] Add missing `tan` call for ackermann (#1117) --- steering_controllers_library/src/steering_odometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 32cdb69454..0d12518a3d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -123,7 +123,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -136,7 +136,7 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -148,7 +148,7 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } From dc928985b34c8bf439bc92f210d26209d09f262c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 1 Jul 2024 13:52:30 +0200 Subject: [PATCH 03/27] Bump version of pre-commit hooks (#1185) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c75acbd76d..29862f70e4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.15.2 + rev: v3.16.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -56,14 +56,14 @@ repos: args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.0.0 + rev: 7.1.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.5 + rev: v18.1.8 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.4 + rev: 0.28.6 hooks: - id: check-github-workflows args: ["--verbose"] From 6439a0ded6d0072af0be710cd371a6897e5924a9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:03:41 +0100 Subject: [PATCH 04/27] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 7 +++++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 7 +++++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/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 | 3 +++ steering_controllers_library/CHANGELOG.rst | 8 ++++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 7 +++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 84 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 5cfdefb83b..165573dc84 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1a2dd8ed96..ce1aa256f1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 8c3d27e24f..52c41f549c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a4473f0f83..6be80a2e54 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 63b44cd218..eb619ca1ba 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index deca56a197..005a23c0e4 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 +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3f1917dcad..1db9c16874 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e0b5a0a311..e7a8105cd2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index cebfc8d78c..96141e51e1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ec9b79726f..8582c56d4f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 544cf500ef..4d7071701b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove manual angle-wraparound parameter (`#1152 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * JTC trajectory end time validation fix (`#1090 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 4b884dc386..58bdcd3691 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index f454aadc4e..85c48140ff 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7dd295970d..a689b4fda5 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index af872ec8a3..5fcde17af9 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 19ead3093f..99553224b7 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 +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 121a5066c2..96cab6e11c 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * [RQT-JTC] limits from jtc controlled joints (`#1146 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e1cdc069df..8a16a89fc4 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Enrique Llorente Pastora, Quique Llorente + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 8f36bae23c..231774c823 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f6423146a0..901ce56631 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 64a09e4309..ca954bb079 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ From b441ced4c19dba79067239e7a28751adad298640 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:04:06 +0100 Subject: [PATCH 05/27] 4.10.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 +- 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 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/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 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/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 +- 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 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 165573dc84..e27cac2205 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 7b66dd765b..77555c1f01 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.9.0 + 4.10.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ce1aa256f1..a3faf30ab1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2add40baf7..9abdb5cf82 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.9.0 + 4.10.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 52c41f549c..a8956560b5 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index bcad405abe..b49b9f1a23 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6be80a2e54..5b9275359d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8765d2506f..9cfdee9a29 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.9.0 + 4.10.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index eb619ca1ba..9b1240263b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6519a07b07..faf31ada51 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 005a23c0e4..8f978cfd08 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4576bbb8b0..ae0c89dcf2 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 1db9c16874..75926feff6 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index b4f34fce8c..1e24ca0201 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e7a8105cd2..adf9f8e208 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index f4ee00591a..5a9ff6ed87 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.9.0 + 4.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 96141e51e1..6649777076 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 032e5addc1..383c90d89a 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8582c56d4f..e4f73e5cc6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a34c05b269..9ca30f4e52 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.9.0 + 4.10.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 4d7071701b..2c044b28b0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove manual angle-wraparound parameter (`#1152 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 2c04d7fa23..733059736a 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.9.0 + 4.10.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 58bdcd3691..101d507f56 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index be598f94ae..3bd87c1875 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.9.0 + 4.10.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 85c48140ff..0582a40895 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 37bcdbe108..013b60c790 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index a689b4fda5..7c671ae3d3 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 31e8b1755b..da74b2d62f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5fcde17af9..1db9ae1c05 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 278ecb0b1a..14f686cce4 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.9.0 + 4.10.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 99553224b7..c483ee7c2f 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c321c9703d..2c26e5cfe0 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.9.0 + 4.10.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e0dfb4d65e..bb9ff7b303 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.9.0", + version="4.10.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 96cab6e11c..bd2b00a1fe 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 68a173217a..4d42c2d00d 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 - 4.9.0 + 4.10.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 e9a19034c6..b966635801 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.9.0", + version="4.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8a16a89fc4..ee21fed403 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 08d252d794..5b55f7f9de 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.9.0 + 4.10.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 231774c823..4034036e08 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 51fe867106..cdb62117fd 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.9.0 + 4.10.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 901ce56631..c80b90ff0f 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 6e1ed66edd..f2408123df 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ca954bb079..c4d146a21d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 05f6245686..75c05fab94 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From df0449263f1568c4340a4e712ffb8d8412e06a0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:43:19 +0200 Subject: [PATCH 06/27] Fix steering controllers library kinematics (#1150) --- .../test_ackermann_steering_controller.cpp | 6 +- .../test/test_bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 17 ++- .../src/steering_controllers_library.cpp | 2 +- .../src/steering_odometry.cpp | 82 ++++++++--- .../test/test_steering_odometry.cpp | 127 +++++++++++++++--- .../test_tricycle_steering_controller.cpp | 3 + 7 files changed, 197 insertions(+), 50 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 38cca0cac9..718e6f5856 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR( + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( @@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index fd8565853f..9904f791b6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -237,7 +237,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command[0], 1.1); EXPECT_EQ(msg.steering_angle_command[0], 2.2); - publish_commands(); + publish_commands(0.1, 0.2); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( @@ -245,14 +245,14 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } 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 0104b011c7..dadc5730d4 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,6 +18,7 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#include #include #include @@ -36,6 +37,9 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -188,10 +192,11 @@ class SteeringOdometry * \brief Calculates inverse kinematics for the desired linear and angular velocities * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz); + const double v_bx, const double omega_bz, const bool open_loop = true); /** * \brief Reset poses, heading, and accumulators @@ -230,6 +235,16 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + /** * \brief Reset linear and angular accumulators */ diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5445196a6e..1aef556212 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -401,7 +401,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_angular_velocity_ = reference_interfaces_[1]; auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0d12518a3d..f8b42771e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace steering_odometry { @@ -128,13 +129,26 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } +double SteeringOdometry::get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + // overdetermined, we take the average + double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius + wheel_track_ * 0.5); + double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; +} + bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; @@ -145,10 +159,18 @@ bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -181,30 +203,41 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (omega_bz == 0 || v_bx == 0) + if (fabs(v_bx) < std::numeric_limits::epsilon()) { - return 0; + // avoid division by zero + return 0.; } return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz) + const double v_bx, const double omega_bz, const bool open_loop) { // desired wheel speed and steering angle of the middle of traction and steering axis - double Ws, phi; + double Ws, phi, phi_IK = steer_pos_; +#if 0 if (v_bx == 0 && omega_bz != 0) { - // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + // TODO(anyone) this would be only possible if traction is on the steering axis phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); - Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; } + // wheel speed + Ws = v_bx / wheel_radius_; if (config_type_ == BICYCLE_CONFIG) { @@ -216,17 +249,20 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } + // simple steering steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } @@ -234,14 +270,16 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; + // shortcut, no steering steering_commands = {phi, phi}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; @@ -279,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2( const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(theta_mid) * dt; - y_ += v_bx * sin(theta_mid) * dt; + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; heading_ += omega_bz * dt; } @@ -289,7 +327,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double delta_x_b = v_bx * dt; const double delta_theta = omega_bz * dt; - if (fabs(delta_theta) < 1e-6) + if (is_close_to_zero(delta_theta)) { /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): integrate_runge_kutta_2(v_bx, omega_bz, dt); @@ -300,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double heading_old = heading_; const double R = delta_x_b / delta_theta; heading_ += delta_theta; - x_ += R * (sin(heading_) - sin(heading_old)); - y_ += -R * (cos(heading_) - cos(heading_old)); + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } } diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..d93e29eca5 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,13 +78,13 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); - auto cmd = odom.get_commands(1., 0.); + auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -79,13 +93,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., 0.1); + auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -94,13 +108,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., -0.1); + auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); @@ -109,6 +123,47 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -121,25 +176,57 @@ TEST(TestSteeringOdometry, bicycle_odometry) EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } -TEST(TestSteeringOdometry, tricycle_odometry) +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) { steering_odometry::SteeringOdometry odom(1); - odom.set_wheel_params(1., 1., 1.); + odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); - EXPECT_NEAR(odom.get_angular(), .1, 1e-3); - EXPECT_NEAR(odom.get_x(), .1, 1e-3); - EXPECT_NEAR(odom.get_heading(), .01, 1e-3); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering } -TEST(TestSteeringOdometry, ackermann_odometry) +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..328f5e4d6a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -201,6 +201,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -246,6 +247,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -258,6 +260,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( From 07061f96f21fd4436a1f48b29e74beb21be8709a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:56:47 +0200 Subject: [PATCH 07/27] [JTC] Process tolerances sent with action goal (#716) --- doc/migration/Jazzy.rst | 1 + doc/release_notes/Jazzy.rst | 14 + joint_trajectory_controller/CMakeLists.txt | 4 + joint_trajectory_controller/doc/userdoc.rst | 16 +- .../joint_trajectory_controller.hpp | 3 + .../tolerances.hpp | 219 ++++++++- .../src/joint_trajectory_controller.cpp | 55 ++- .../test/test_tolerances.cpp | 424 ++++++++++++++++++ .../test/test_trajectory_actions.cpp | 178 +++++++- .../test/test_trajectory_controller_utils.hpp | 41 +- 10 files changed, 921 insertions(+), 34 deletions(-) create mode 100644 joint_trajectory_controller/test/test_tolerances.cpp diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst index 72e6a52e6d..2114436098 100644 --- a/doc/migration/Jazzy.rst +++ b/doc/migration/Jazzy.rst @@ -18,3 +18,4 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. +* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4c5db18f3e..4793fd957d 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -33,6 +33,20 @@ joint_trajectory_controller * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. pid_controller ************************ diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..46c245be9c 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -61,6 +61,10 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index af495ad14d..4dcb71a064 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -152,7 +152,21 @@ Actions [#f1]_ The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: + +.. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. 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 ae370df0a6..c3fbb58456 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 @@ -244,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c46b1c297f..1998930182 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,12 +30,15 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" namespace joint_trajectory_controller { @@ -85,16 +88,19 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -106,16 +112,221 @@ SegmentTolerances get_segment_tolerances(Params const & params) tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param jtc_logger The logger to use for output + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param joints The joints configured by ROS parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances(default_tolerances); + + active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (joint_tol.position > 0.0) + { + active_tolerances.state_tolerance[i].position = joint_tol.position; + } + else if (is_erase_value(joint_tol.position)) + { + active_tolerances.state_tolerance[i].position = 0.0; + } + else if (joint_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.velocity > 0.0) + { + active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; + } + else if (is_erase_value(joint_tol.velocity)) + { + active_tolerances.state_tolerance[i].velocity = 0.0; + } + else if (joint_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.acceleration > 0.0) + { + active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; + } + else if (is_erase_value(joint_tol.acceleration)) + { + active_tolerances.state_tolerance[i].acceleration = 0.0; + } + else if (joint_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (goal_tol.position > 0.0) + { + active_tolerances.goal_state_tolerance[i].position = goal_tol.position; + } + else if (is_erase_value(goal_tol.position)) + { + active_tolerances.goal_state_tolerance[i].position = 0.0; + } + else if (goal_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.velocity > 0.0) + { + active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; + } + else if (is_erase_value(goal_tol.velocity)) + { + active_tolerances.goal_state_tolerance[i].velocity = 0.0; + } + else if (goal_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.acceleration > 0.0) + { + active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; + } + else if (is_erase_value(goal_tol.acceleration)) + { + active_tolerances.goal_state_tolerance[i].acceleration = 0.0; + } + else if (goal_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b80d1b7908..31598bb813 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -126,11 +126,12 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) @@ -200,6 +201,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -207,7 +209,7 @@ controller_interface::return_type JointTrajectoryController::update( !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -224,8 +226,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], - true /* show_errors */)) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -233,14 +234,14 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], - false /* show_errors */)) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; // print once, goal will be aborted afterwards @@ -320,7 +321,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -339,7 +340,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); @@ -358,7 +359,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); + RCLCPP_WARN(logger, error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -368,7 +369,7 @@ controller_interface::return_type JointTrajectoryController::update( else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -376,7 +377,7 @@ controller_interface::return_type JointTrajectoryController::update( else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -620,11 +621,11 @@ void JointTrajectoryController::query_state_service( controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -782,6 +783,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -873,6 +876,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -880,7 +885,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( params_ = param_listener_->get_params(); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // order all joints in the storage for (const auto & interface : params_.command_interfaces) @@ -892,8 +897,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -906,8 +911,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -949,9 +954,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // deactivate timeout RCLCPP_WARN( - get_node()->get_logger(), - "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, - default_tolerances_.goal_time_tolerance); + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); cmd_timeout_ = 0.0; } } @@ -1173,6 +1177,11 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..66914b6da4 --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,424 @@ +// Copyright 2024 Austrian Institute of Technology +// +// 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 "gmock/gmock.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double timeout, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 332a30c53a..fb749ac250 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -31,7 +31,6 @@ #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -44,10 +43,12 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -152,10 +153,16 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -488,6 +495,165 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SCOPED_TRACE("Check default values"); + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } +} + TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters @@ -682,7 +848,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -732,7 +899,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f049bc65a9..693e19e67a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" namespace { @@ -38,11 +39,44 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); } - } // namespace namespace test_trajectory_controllers @@ -138,6 +172,11 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + std::vector get_pids() const { return pids_; } joint_trajectory_controller::SegmentTolerances get_tolerances() const From 991ef48b79a12974a1b37763e818842906e3553b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jul 2024 23:08:20 +0200 Subject: [PATCH 08/27] added changes corresponding to the logger and clock propagation in ResourceManager (#1184) --- .../test/test_load_ackermann_steering_controller.cpp | 4 +--- .../test/test_load_admittance_controller.cpp | 4 +--- .../test/test_load_bicycle_steering_controller.cpp | 4 +--- .../test/test_load_diff_drive_controller.cpp | 3 +-- .../test/test_load_joint_group_effort_controller.cpp | 4 +--- .../test/test_load_force_torque_sensor_broadcaster.cpp | 4 +--- .../test/test_load_forward_command_controller.cpp | 4 +--- .../test_load_multi_interface_forward_command_controller.cpp | 4 +--- .../test/test_load_gripper_action_controllers.cpp | 4 +--- .../test/test_load_imu_sensor_broadcaster.cpp | 4 +--- .../test/test_load_joint_state_broadcaster.cpp | 4 +--- .../test/test_load_joint_trajectory_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 5 ++--- .../test/test_load_joint_group_position_controller.cpp | 4 +--- .../test/test_load_range_sensor_broadcaster.cpp | 4 +--- tricycle_controller/test/test_load_tricycle_controller.cpp | 4 +--- .../test/test_load_tricycle_steering_controller.cpp | 4 +--- .../test/test_load_joint_group_velocity_controller.cpp | 4 +--- 18 files changed, 19 insertions(+), 53 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 0a8cd7b80c..101ecce8ff 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 23be1f23f5..69808f3505 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,9 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 955feb33c5..f3828660a5 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 4c9d2f984f..0c65532c38 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,8 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique(ros2_control_test_assets::diffbot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 52f1f9934a..4008ac8534 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 0c269d6a31..2e87505003 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index b493e52b2a..c192f1eb5f 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 41a9b74698..52b63bdae8 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 0ef5f0bcb2..5641e1b4b3 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,9 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index f4e6105ed6..a477a731ff 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 5efb587805..266e33c1c8 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index eb1a3691e6..b3635efaca 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 3a75f6e170..ed645b872c 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,9 +33,8 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index bc27b5e629..950773351c 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 5c400bef91..1d8a55b932 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bd54459780..bb4194909c 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,9 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 0d04afdf38..4fa3698409 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index e426349f96..7070594639 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 776c432dbc9a8db11119823803daad9525c63846 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 9 Jul 2024 09:08:28 +0200 Subject: [PATCH 09/27] [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (#1192) --- .../tolerances.hpp | 186 +++++++----------- .../test/test_tolerances.cpp | 35 +++- .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 99 insertions(+), 123 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 1998930182..c4404920df 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -31,6 +31,7 @@ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #include +#include #include #include @@ -126,6 +127,33 @@ SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Para return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + /** * \brief Populate trajectory segment tolerances using data from an action goal. * @@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances( const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); - - active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); static auto logger = jtc_logger.get_child("tolerance"); - RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); - // from - // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg - // There are two special values for tolerances: - // * 0 - The tolerance is unspecified and will remain at whatever the default is - // * -1 - The tolerance is "erased". - // If there was a default, the joint will be allowed to move without restriction. - constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) - { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "Specified illegal goal_time_tolerance: " + << rclcpp::Duration(goal.goal_time_tolerance).seconds() + << ". Using default tolerances"); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -173,60 +203,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (joint_tol.position > 0.0) - { - active_tolerances.state_tolerance[i].position = joint_tol.position; - } - else if (is_erase_value(joint_tol.position)) + std::string interface = ""; + try { - active_tolerances.state_tolerance[i].position = 0.0; + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); } - else if (joint_tol.position < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.velocity > 0.0) - { - active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; - } - else if (is_erase_value(joint_tol.velocity)) - { - active_tolerances.state_tolerance[i].velocity = 0.0; - } - else if (joint_tol.velocity < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.acceleration > 0.0) - { - active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; - } - else if (is_erase_value(joint_tol.acceleration)) - { - active_tolerances.state_tolerance[i].acceleration = 0.0; - } - else if (joint_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } @@ -256,60 +250,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (goal_tol.position > 0.0) - { - active_tolerances.goal_state_tolerance[i].position = goal_tol.position; - } - else if (is_erase_value(goal_tol.position)) - { - active_tolerances.goal_state_tolerance[i].position = 0.0; - } - else if (goal_tol.position < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.velocity > 0.0) - { - active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; - } - else if (is_erase_value(goal_tol.velocity)) + std::string interface = ""; + try { - active_tolerances.goal_state_tolerance[i].velocity = 0.0; + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); } - else if (goal_tol.velocity < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.acceleration > 0.0) - { - active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; - } - else if (is_erase_value(goal_tol.acceleration)) - { - active_tolerances.goal_state_tolerance[i].acceleration = 0.0; - } - else if (goal_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 66914b6da4..af5036b869 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -183,7 +183,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) path_tolerance.push_back(tolerance); goal_tolerance.push_back(tolerance); - auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); @@ -215,6 +215,31 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) // send goal with invalid tolerances, are the default ones used? TEST_F(TestTolerancesFixture, test_invalid_tolerances) { + { + SCOPED_TRACE("negative goal_time_tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } { SCOPED_TRACE("negative path position tolerance"); std::vector points; @@ -238,7 +263,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -264,7 +288,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -290,7 +313,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } } @@ -395,7 +414,6 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) @@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 693e19e67a..bbe12251a1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -45,6 +45,7 @@ const double stopped_velocity_tolerance = 0.1; [[maybe_unused]] void expectDefaultTolerances( joint_trajectory_controller::SegmentTolerances active_tolerances) { + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From a0d844f053d22b19045f51c38a2fa09d98148c89 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:14 +0100 Subject: [PATCH 10/27] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 21 files changed, 104 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e27cac2205..84ac53044c 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a3faf30ab1..e75a71a8af 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a8956560b5..b1e1bf832f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5b9275359d..c402e58344 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 9b1240263b..6f7c2771cb 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8f978cfd08..a7392ad187 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 75926feff6..d37dd5f30d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index adf9f8e208..acd4c2534c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6649777076..46ae9e4d27 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e4f73e5cc6..f20ca59557 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2c044b28b0..23838b7e3f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* [JTC] Process tolerances sent with action goal (`#716 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove manual angle-wraparound parameter (`#1152 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 101d507f56..d5d140339b 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 0582a40895..d5f8b52cbb 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7c671ae3d3..6be8a70b9d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1db9ae1c05..39c00e3c85 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c483ee7c2f..d529a740b3 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 +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index bd2b00a1fe..2ffbb95666 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ee21fed403..7ea9c5d893 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich + 4.10.0 (2024-07-01) ------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 4034036e08..feb9b105cb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index c80b90ff0f..a34e991d2a 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c4d146a21d..e749d6f8a0 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- From 18ce11a42d1549678b418794d283ea80914c4a6b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:31 +0100 Subject: [PATCH 11/27] 4.11.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 +- 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 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/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 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/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 +- 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 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 84ac53044c..21840061d4 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 77555c1f01..dfa733515f 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.10.0 + 4.11.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index e75a71a8af..4ae5ea39f8 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9abdb5cf82..d6e13b0634 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.10.0 + 4.11.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index b1e1bf832f..831aa5a2b3 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index b49b9f1a23..7afcf53706 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c402e58344..00f777df67 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9cfdee9a29..d7fa06b762 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.10.0 + 4.11.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6f7c2771cb..70cfaad021 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index faf31ada51..a7744927a1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a7392ad187..eac8c9959a 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 ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index ae0c89dcf2..90fcb3a684 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d37dd5f30d..17196fed65 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1e24ca0201..f4c02d8de7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index acd4c2534c..968a1088c2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 5a9ff6ed87..72d2ec48f2 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.10.0 + 4.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 46ae9e4d27..5bacde755e 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 383c90d89a..5f0e981605 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f20ca59557..cc01373289 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9ca30f4e52..77bbe2ca00 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.10.0 + 4.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 23838b7e3f..047d232155 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * [JTC] Process tolerances sent with action goal (`#716 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 733059736a..d32d4e3daf 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.10.0 + 4.11.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index d5d140339b..8e45f042f0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 3bd87c1875..2a44c9ad1b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.10.0 + 4.11.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d5f8b52cbb..c9e2c749b5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 013b60c790..33f0f0c15e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 6be8a70b9d..31458c07d6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index da74b2d62f..e4525f53fd 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 39c00e3c85..a54d39b85d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 14f686cce4..24e4429ade 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.10.0 + 4.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d529a740b3..78e18dffaa 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 ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2c26e5cfe0..87af6e6695 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.10.0 + 4.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index bb9ff7b303..91db299739 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.10.0", + version="4.11.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 2ffbb95666..20450f3ca3 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 ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4d42c2d00d..85c44c58fe 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 - 4.10.0 + 4.11.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 b966635801..2588a47cc8 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.10.0", + version="4.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 7ea9c5d893..d4f62b806c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5b55f7f9de..7d172d45a8 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.10.0 + 4.11.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index feb9b105cb..fdde4481e2 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cdb62117fd..20310e6b9a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.10.0 + 4.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a34e991d2a..6f4ad5d8c1 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f2408123df..4aeb8a086f 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e749d6f8a0..614e023984 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 75c05fab94..d128f2b45b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 91ebb73b23872f60e09a0d3826b57d0f8b3bf5b7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 11 Jul 2024 07:38:47 -0700 Subject: [PATCH 12/27] Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (#1002) --- doc/controllers_index.rst | 1 + .../gripper_action_controller_impl.hpp | 5 + parallel_gripper_controller/CMakeLists.txt | 84 ++++ parallel_gripper_controller/doc/userdoc.rst | 19 + .../parallel_gripper_action_controller.hpp | 174 ++++++++ ...arallel_gripper_action_controller_impl.hpp | 396 ++++++++++++++++++ .../visibility_control.hpp | 56 +++ parallel_gripper_controller/package.xml | 35 ++ .../ros_control_plugins.xml | 10 + .../gripper_action_controller_parameters.yaml | 69 +++ .../parallel_gripper_action_controller.cpp | 25 ++ ...oad_parallel_gripper_action_controller.cpp | 45 ++ .../test/test_parallel_gripper_controller.cpp | 169 ++++++++ .../test/test_parallel_gripper_controller.hpp | 66 +++ 14 files changed, 1154 insertions(+) create mode 100644 parallel_gripper_controller/CMakeLists.txt create mode 100644 parallel_gripper_controller/doc/userdoc.rst create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp create mode 100644 parallel_gripper_controller/package.xml create mode 100644 parallel_gripper_controller/ros_control_plugins.xml create mode 100644 parallel_gripper_controller/src/gripper_action_controller_parameters.yaml create mode 100644 parallel_gripper_controller/src/parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.hpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2cb55150ce..78abe5d21f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -52,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/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> diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 77598591ae..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() template controller_interface::CallbackReturn GripperActionController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `position_controllers/GripperActionController` and " + "`effort_controllers::GripperActionController` controllers are replaced by " + "'parallel_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..56aa623a6f --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +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(parallel_gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + +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 + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5c04eb5f90 --- /dev/null +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst + +.. _parallel_gripper_controller_userdoc: + +Parallel Gripper Action Controller +----------------------------------- + +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp new file mode 100644 index 0000000000..d303990b89 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -0,0 +1,174 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/parallel_gripper_command.hpp" + +// rclcpp_action +#include "rclcpp_action/create_server.hpp" + +// ros_controls +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "parallel_gripper_action_controller_parameters.hpp" + +namespace parallel_gripper_action_controller +{ +/** + * \brief Controller for executing a `ParallelGripperCommand` action. + * + * \tparam HardwareInterface Controller hardware interface. Currently \p + * hardware_interface::HW_IF_POSITION and \p + * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. + */ + +class GripperActionController : public controller_interface::ControllerInterface +{ +public: + /** + * \brief Store position and max effort in struct to allow easier realtime + * buffer usage + */ + struct Commands + { + double position_cmd_; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the + * position and velocity state interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; + using ActionServer = rclcpp_action::Server; + using ActionServerPtr = ActionServer::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + bool update_hold_position_; + + bool verbose_ = false; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::optional> + joint_command_interface_; + std::optional> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; + + rclcpp::Duration action_monitor_period_; + + // ROS API + ActionServerPtr action_server_; + + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + + void accepted_callback(std::shared_ptr goal_handle); + + void preempt_active_goal(); + + void set_hold_position(); + + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command + + /** + * \brief Check for success and publish appropriate result and feedback. + **/ + void check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); +}; + +} // namespace parallel_gripper_action_controller + +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..b6fccf1bbf --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -0,0 +1,396 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include +#include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace parallel_gripper_action_controller +{ + +void GripperActionController::preempt_active_goal() +{ + // Cancels the currently active goal + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + // Marks the current goal as canceled + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + command_struct_rt_ = *(command_.readFromRT()); + + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double error_position = command_struct_rt_.position_cmd_ - current_position; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) +{ + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionController::accepted_callback( + std::shared_ptr goal_handle) // Try to update goal +{ + auto rt_goal = std::make_shared(goal_handle); + + // Accept new goal + preempt_active_goal(); + + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + { + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + } + else + { + command_struct_.max_velocity_ = params_.max_velocity; + } + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } + command_.writeFromNonRT(command_struct_); + + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +rclcpp_action::CancelResponse GripperActionController::cancel_callback( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Enter hold current position mode + set_hold_position(); + + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + // Reset current goal + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.writeFromNonRT(command_struct_); +} + +void GripperActionController::check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) + { + return; + } + + if (fabs(error_position) < params_.goal_tolerance) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = true; + pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + else + { + if (fabs(current_velocity) > params_.stall_velocity_threshold) + { + last_movement_time_ = time; + } + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = true; + + if (params_.allow_stalling) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + active_goal->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + active_goal->setAborted(pre_alloc_result_); + } + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + // Action status checking update rate + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + RCLCPP_INFO_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) +{ + auto command_interface_it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto position_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (position_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (position_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto velocity_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); + if (velocity_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (velocity_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + + joint_command_interface_ = *command_interface_it; + joint_position_state_interface_ = *position_state_interface_it; + joint_velocity_state_interface_ = *velocity_state_interface_it; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "set_gripper_max_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "set_gripper_max_velocity") + { + speed_interface_ = interface; + } + } + + + // Command - non RT version + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + // Action interface + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_cmd", + std::bind( + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), + std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + joint_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (!params_.max_effort_interface.empty()) + { + names.push_back({params_.max_effort_interface}); + } + if (!params_.max_velocity_interface.empty()) + { + names.push_back({params_.max_velocity_interface}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace parallel_gripper_action_controller + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..63e5c8e1c7 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml new file mode 100644 index 0000000000..f189071617 --- /dev/null +++ b/parallel_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + parallel_gripper_controller + 2.32.0 + The parallel_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9b0dbb8a05 --- /dev/null +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,69 @@ +parallel_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + read_only: true, + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + read_only: true, + } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 0.0 ] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, + } diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..3a6d028ddf --- /dev/null +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -0,0 +1,25 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta + +// Project +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + parallel_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..130b12e0bb --- /dev/null +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -0,0 +1,45 @@ +// 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/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGripperActionControllers, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); + + rclcpp::shutdown(); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp new file mode 100644 index 0000000000..d9a5ae958b --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -0,0 +1,169 @@ +// 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 + +#include "gmock/gmock.h" + +#include "test_parallel_gripper_controller.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::ParallelGripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +void GripperControllerTest::SetUpController() +{ + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp new file mode 100644 index 0000000000..8bc6ab954c --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 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. + +#ifndef TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController : public parallel_gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ From ebca6c0a3f29d75521a7a85683206b32073bd8ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jul 2024 12:02:19 +0200 Subject: [PATCH 13/27] Fix parallel gripper controller CI (#1202) --- .../test/test_load_ackermann_steering_controller.cpp | 2 +- .../test/test_load_admittance_controller.cpp | 2 +- .../test/test_load_bicycle_steering_controller.cpp | 2 +- .../test/test_load_diff_drive_controller.cpp | 2 +- .../test/test_load_joint_group_effort_controller.cpp | 2 +- .../test/test_load_force_torque_sensor_broadcaster.cpp | 2 +- .../test_load_multi_interface_forward_command_controller.cpp | 2 +- .../test/test_load_gripper_action_controllers.cpp | 2 +- .../test/test_load_imu_sensor_broadcaster.cpp | 2 +- .../test/test_load_joint_state_broadcaster.cpp | 2 +- .../test/test_load_joint_trajectory_controller.cpp | 2 +- .../parallel_gripper_action_controller_impl.hpp | 1 - .../test/test_load_parallel_gripper_action_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 3 +-- .../test/test_load_joint_group_position_controller.cpp | 2 +- .../test/test_load_range_sensor_broadcaster.cpp | 2 +- tricycle_controller/test/test_load_tricycle_controller.cpp | 2 +- .../test/test_load_tricycle_steering_controller.cpp | 2 +- .../test/test_load_joint_group_velocity_controller.cpp | 2 +- 19 files changed, 18 insertions(+), 22 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 101ecce8ff..8e81643835 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 69808f3505..ea4c14caac 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,7 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index f3828660a5..7be5e2dd1d 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 0c65532c38..bcd334631f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 4008ac8534..0ac1053d18 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 2e87505003..f8b367d341 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 52b63bdae8..49bc277824 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 5641e1b4b3..b607fb6fe7 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,7 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index a477a731ff..fa2afab200 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 266e33c1c8..b3cf778063 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index b3635efaca..5b36afc4e8 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index b6fccf1bbf..bcfbb3b5c5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -322,7 +322,6 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - non RT version command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 130b12e0bb..6ba99ac248 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index ed645b872c..5cfb04e860 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,8 +33,7 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index 950773351c..f71474bd4a 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 1d8a55b932..b416ac4ef9 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bb4194909c..486c04515b 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,7 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 4fa3698409..210153eef8 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 7070594639..a7dc72f337 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 362067bc269024faa7a866d13dfc17050b390ce8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 Jul 2024 23:56:03 +0200 Subject: [PATCH 14/27] Use a single file for migration + release notes (#1204) --- doc/{migration/Jazzy.rst => migration.rst} | 0 doc/{release_notes/Jazzy.rst => release_notes.rst} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename doc/{migration/Jazzy.rst => migration.rst} (100%) rename doc/{release_notes/Jazzy.rst => release_notes.rst} (100%) diff --git a/doc/migration/Jazzy.rst b/doc/migration.rst similarity index 100% rename from doc/migration/Jazzy.rst rename to doc/migration.rst diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes.rst similarity index 100% rename from doc/release_notes/Jazzy.rst rename to doc/release_notes.rst From 12b531082bcea252d1c4359dacf648afa3a9568d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jul 2024 11:47:00 +0200 Subject: [PATCH 15/27] [doc] Fix controller idx page (#1205) * Fix controller idx page * Fix gh links --- doc/controllers_index.rst | 4 ++-- doc/migration.rst | 4 ++-- doc/release_notes.rst | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 78abe5d21f..683f23c202 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -16,9 +16,9 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - :glob: - * + mobile_robot_kinematics.rst + writing_new_controller.rst Controllers for Wheeled Mobile Robots diff --git a/doc/migration.rst b/doc/migration.rst index 2114436098..4c0e4608d6 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst -Iron to Jazzy +Migration Guides: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4793fd957d..b55db861b3 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst -Iron to Jazzy +Release Notes: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes the changes between Iron (previous) and Jazzy (current) releases. From 0bb88805713fb21cddc418bc4c502960d9fd9f39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 16 Jul 2024 10:02:42 +0200 Subject: [PATCH 16/27] [JTC] Fix test_tolerances_via_actions (#1209) --- joint_trajectory_controller/test/test_tolerances.cpp | 4 ++-- .../test/test_trajectory_actions.cpp | 11 +++++------ .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index af5036b869..bd6304df29 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -32,14 +32,14 @@ using trajectory_msgs::msg::JointTrajectoryPoint; std::vector joint_names_ = {"joint1", "joint2", "joint3"}; control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( - const std::vector & points, double timeout, + const std::vector & points, double goal_time_tolerance, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fb749ac250..3e65016403 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -153,14 +153,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector & points, double goal_time_tolerance, + const GoalOptions & opt, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; @@ -529,7 +530,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -539,7 +540,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } @@ -639,7 +639,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -649,7 +649,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bbe12251a1..1750f7d30d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -46,6 +46,7 @@ const double stopped_velocity_tolerance = 0.1; joint_trajectory_controller::SegmentTolerances active_tolerances) { EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From b958da39e04ac9a560b68f4361807dcdc0c77ec6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 11:24:08 +0200 Subject: [PATCH 17/27] [GripperController] Fix failing tests (#1210) --- parallel_gripper_controller/CMakeLists.txt | 14 ++--- ...arallel_gripper_action_controller_impl.hpp | 2 +- .../gripper_action_controller_parameters.yaml | 3 ++ .../gripper_action_controller_params.yaml | 7 +++ ...oad_parallel_gripper_action_controller.cpp | 16 +++--- .../test/test_parallel_gripper_controller.cpp | 54 +++++++++---------- .../test/test_parallel_gripper_controller.hpp | 3 +- 7 files changed, 57 insertions(+), 42 deletions(-) create mode 100644 parallel_gripper_controller/test/gripper_action_controller_params.yaml diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 56aa623a6f..75380a953f 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -48,17 +48,19 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_parallel_gripper_action_controllers - test/test_load_parallel_gripper_action_controller.cpp - ) + add_rostest_with_parameters_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) ament_target_dependencies(test_load_parallel_gripper_action_controllers controller_manager ros2_control_test_assets ) - ament_add_gmock(test_parallel_gripper_controller - test/test_parallel_gripper_controller.cpp - ) + add_rostest_with_parameters_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_parallel_gripper_controller PRIVATE include) target_link_libraries(test_parallel_gripper_controller parallel_gripper_action_controller ) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index bcfbb3b5c5..d37cb87fc8 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -44,7 +44,6 @@ controller_interface::CallbackReturn GripperActionController::on_init() try { param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -245,6 +244,7 @@ controller_interface::CallbackReturn GripperActionController::on_configure( RCLCPP_ERROR(logger, "Joint name cannot be empty"); return controller_interface::CallbackReturn::ERROR; } + RCLCPP_INFO(logger, "Joint name is : %s", params_.joint.c_str()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml index 9b0dbb8a05..10b0df0958 100644 --- a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -11,6 +11,9 @@ parallel_gripper_action_controller: joint: { type: string, read_only: true, + validation: { + not_empty<>: null + } } state_interfaces: { type: string_array, diff --git a/parallel_gripper_controller/test/gripper_action_controller_params.yaml b/parallel_gripper_controller/test/gripper_action_controller_params.yaml new file mode 100644 index 0000000000..521960e18c --- /dev/null +++ b/parallel_gripper_controller/test/gripper_action_controller_params.yaml @@ -0,0 +1,7 @@ +test_gripper_action_position_controller: + ros__parameters: + joint: "joint1" + +test_gripper_action_position_controller_empty_joint: + ros__parameters: + joint: "" diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 6ba99ac248..35a10a8c31 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -22,8 +22,6 @@ TEST(TestLoadGripperActionControllers, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -32,12 +30,16 @@ TEST(TestLoadGripperActionControllers, load_controller) ASSERT_NE( cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController"), - nullptr); - ASSERT_NE( - cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + "test_gripper_action_position_controller", + "parallel_gripper_action_controller/GripperActionController"), nullptr); +} +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/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index d9a5ae958b..417d87d40f 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -34,9 +34,9 @@ using GoalHandle = rclcpp_action::ServerGoalHandle; using testing::SizeIs; using testing::UnorderedElementsAre; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void GripperControllerTest::SetUpTestCase() {} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +void GripperControllerTest::TearDownTestCase() {} void GripperControllerTest::SetUp() { @@ -46,11 +46,13 @@ void GripperControllerTest::SetUp() void GripperControllerTest::TearDown() { controller_.reset(nullptr); } -void GripperControllerTest::SetUpController() +void GripperControllerTest::SetUpController( + const std::string & controller_name = "test_gripper_action_position_controller", + controller_interface::return_type expected_result = controller_interface::return_type::OK) { const auto result = - controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, expected_result); std::vector command_ifs; command_ifs.emplace_back(this->joint_1_cmd_); @@ -62,7 +64,9 @@ void GripperControllerTest::SetUpController() TEST_F(GripperControllerTest, ParametersNotSet) { - this->SetUpController(); + this->SetUpController( + "test_gripper_action_position_controller_no_parameters", + controller_interface::return_type::ERROR); // configure failed, 'joints' parameter not set ASSERT_EQ( @@ -72,9 +76,9 @@ TEST_F(GripperControllerTest, ParametersNotSet) TEST_F(GripperControllerTest, JointParameterIsEmpty) { - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", ""}); + this->SetUpController( + "test_gripper_action_position_controller_empty_joint", + controller_interface::return_type::ERROR); // configure failed, 'joints' is empty ASSERT_EQ( @@ -86,7 +90,9 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) { this->SetUpController(); - this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + rclcpp::spin_some(this->controller_->get_node()->get_node_base_interface()); // configure successful ASSERT_EQ( @@ -98,29 +104,14 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); ASSERT_THAT( cmd_if_conf.names, - UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + UnorderedElementsAre(std::string("joint1/") + hardware_interface::HW_IF_POSITION)); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = this->controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); - ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint1/position", "joint1/velocity")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); - - // activate failed, 'joint4' is not a valid joint name for the hardware - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - TEST_F(GripperControllerTest, ActivateSuccess) { this->SetUpController(); @@ -167,3 +158,12 @@ TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } + +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/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index 8bc6ab954c..d206097782 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -42,7 +42,8 @@ class GripperControllerTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpController(); + void SetUpController( + const std::string & controller_name, controller_interface::return_type expected_result); void SetUpHandles(); protected: From 2674f6d2d1821231781d9528089c7d9d24e76a33 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 14:09:30 +0200 Subject: [PATCH 18/27] Fix WaitSet issue in tests (#1206) --- .../test_ackermann_steering_controller.cpp | 2 +- .../test_ackermann_steering_controller.hpp | 49 +++++++--------- .../test/test_admittance_controller.cpp | 2 +- .../test/test_admittance_controller.hpp | 49 +++++++--------- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 48 ++++++++-------- .../test/test_diff_drive_controller.cpp | 15 ++--- .../test_joint_group_effort_controller.cpp | 23 +++----- .../test_joint_group_effort_controller.hpp | 2 + .../test_force_torque_sensor_broadcaster.cpp | 21 +++++-- .../test/test_forward_command_controller.cpp | 24 +++----- .../test/test_forward_command_controller.hpp | 2 + ...i_interface_forward_command_controller.cpp | 24 +++----- ...i_interface_forward_command_controller.hpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 20 +++++-- .../test/test_joint_state_broadcaster.cpp | 57 ++++++++++++------- .../test/test_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller_utils.hpp | 28 ++++----- pid_controller/test/test_pid_controller.cpp | 2 +- pid_controller/test/test_pid_controller.hpp | 52 ++++++++--------- .../test_joint_group_position_controller.cpp | 23 +++----- .../test_joint_group_position_controller.hpp | 2 + .../test/test_range_sensor_broadcaster.cpp | 20 +++++-- .../test_steering_controllers_library.hpp | 49 +++++++--------- .../test/test_tricycle_controller.cpp | 15 ++--- .../test_tricycle_steering_controller.cpp | 2 +- .../test_tricycle_steering_controller.hpp | 49 ++++++++-------- .../test_joint_group_velocity_controller.cpp | 23 +++----- .../test_joint_group_velocity_controller.hpp | 2 + 29 files changed, 294 insertions(+), 319 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 718e6f5856..de45accc0a 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -257,7 +257,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat EXPECT_EQ(msg.steering_angle_command[1], 4.4); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 363db793d5..9379d956c9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -76,14 +76,7 @@ class TestableAckermannSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -97,30 +90,25 @@ class TestableAckermannSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -214,36 +202,43 @@ class AckermannSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 6b03249df8..256fe1e5fe 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -277,7 +277,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); broadcast_tfs(); ASSERT_EQ( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 19908d7f9f..a464d050be 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -57,15 +57,6 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} - } // namespace // subclassing and friending so we can access member variables @@ -92,39 +83,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); - } - return ret; + return admittance_controller::AdmittanceController::on_configure(previous_state); } /** * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = - input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } private: - rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; - rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -279,9 +258,12 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + ControllerStateMsg::SharedPtr received_msg; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/status", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node_->get_node_base_interface()); // call update to publish the test value ASSERT_EQ( @@ -289,11 +271,18 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node_->get_clock()->now() + timeout; + while (!received_msg && test_subscription_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands() diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 9904f791b6..a06aabbd20 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -238,7 +238,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(0.1, 0.2); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index dcdb7ed169..4643ee3ee9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -74,13 +74,7 @@ class TestableBicycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -94,30 +88,25 @@ class TestableBicycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -187,34 +176,43 @@ class BicycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on max_sub_check_loop_count - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f3f99ac8d8..4976da4dfa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -57,22 +57,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } /** @@ -550,7 +545,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 200a1beda8..06bbc6fd22 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupEffortControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + 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( diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..e871ac6c43 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -24,6 +24,7 @@ #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; @@ -57,6 +58,7 @@ class JointGroupEffortControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2412361352..9528adbb0f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -75,31 +75,40 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg) { // create a new subscriber + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); + wrench_msg = *received_msg; } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index c31c8a964c..fcfa65ee1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -30,24 +30,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void ForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -71,6 +59,7 @@ void ForwardCommandControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) @@ -320,10 +309,13 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + 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( diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..2284d46d61 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -24,6 +24,7 @@ #include "forward_command_controller/forward_command_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_POSITION; @@ -69,6 +70,7 @@ class ForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7879d5c1d7..c52f4f3ab6 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -32,24 +32,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void MultiInterfaceForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -74,6 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params command_ifs.emplace_back(joint_1_vel_cmd_); command_ifs.emplace_back(joint_1_eff_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); if (set_params_and_activate) { @@ -273,10 +262,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + 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( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..78b244847b 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -26,6 +26,7 @@ #include "forward_command_controller/multi_interface_forward_command_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; @@ -77,6 +78,7 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 25a39a8b4d..e8aa571112 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -77,31 +77,39 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber + sensor_msgs::msg::Imu::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(imu_msg, msg_info)); + imu_msg = *received_msg; } TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 2faa55f467..4e4c0631f0 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -664,31 +664,41 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + sensor_msgs::msg::JointState::SharedPtr received_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); + joint_state_msg = *received_msg; } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -731,51 +741,58 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) + { dynamic_joint_state_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + dynamic_joint_state_msg.reset(); + ASSERT_FALSE(dynamic_joint_state_msg); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (dynamic_joint_state_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - - // take message from subscription - control_msgs::msg::DynamicJointState dynamic_joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); + ASSERT_TRUE(dynamic_joint_state_msg); const size_t NUM_JOINTS = 3; const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; - ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, + dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); const auto index_in_original_order = std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].values, + dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 611c1d4c1b..21c5705505 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -253,7 +253,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); + subscribeToState(executor); updateController(); // Spin to receive latest state @@ -644,7 +644,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun SetUpAndActivateTrajectoryController( executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); - subscribeToState(); + subscribeToState(executor); size_t n_joints = joint_names_.size(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 1750f7d30d..a15b2e5bc5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -94,11 +94,6 @@ class TestableJointTrajectoryController const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); - // this class can still be useful without the wait set - if (joint_command_subscriber_) - { - joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); - } return ret; } @@ -108,19 +103,17 @@ class TestableJointTrajectoryController * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout */ - bool wait_for_trajectory( + void wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } void set_joint_names(const std::vector & joint_names) @@ -223,7 +216,6 @@ class TestableJointTrajectoryController } } - rclcpp::WaitSet joint_cmd_sub_wait_set_; rclcpp::NodeOptions node_options_; }; @@ -399,7 +391,7 @@ class TrajectoryControllerTest : public ::testing::Test static void TearDownTestCase() { rclcpp::shutdown(); } - void subscribeToState() + void subscribeToState(rclcpp::Executor & executor) { auto traj_lifecycle_node = traj_controller_->get_node(); @@ -416,6 +408,14 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } } /// Publish trajectory msgs with multiple points diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a44347f5f1..4e9601ae66 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -495,7 +495,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1c356263e7..8c36dca60f 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -65,13 +65,7 @@ class TestablePidController : public pid_controller::PidController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = pid_controller::PidController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; + return pid_controller::PidController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -85,30 +79,25 @@ class TestablePidController : public pid_controller::PidController * @brief wait_for_command blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -182,36 +171,43 @@ class PidControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_pid_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands( diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 60bff556db..a4a288a8e4 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupPositionControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + 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( diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..c7d704db52 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -24,6 +24,7 @@ #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; @@ -58,6 +59,7 @@ class JointGroupPositionControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a23d5e3cde..ef9d5187a3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -66,31 +66,39 @@ controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broad void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) { // create a new subscriber + sensor_msgs::msg::Range::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_range_sensor_broadcaster/range", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(range_msg, msg_info)); + range_msg = *received_msg; } TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b6fe1770c1..ae7c41a09b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -80,14 +80,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -101,26 +94,24 @@ class TestableSteeringControllersLibrary * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } // implementing methods which are declared virtual in the steering_controllers_library.hpp @@ -139,9 +130,6 @@ class TestableSteeringControllersLibrary } bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -235,36 +223,43 @@ class SteeringControllersLibraryFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 4be1680980..901597a8bc 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -62,22 +62,17 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } }; @@ -326,7 +321,7 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); 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.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 328f5e4d6a..ad7f80607f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -241,7 +241,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); 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 b4d58a95c0..8a3ea33925 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -75,14 +75,7 @@ class TestableTricycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -96,30 +89,25 @@ class TestableTricycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + return wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -201,34 +189,43 @@ class TricycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index a99ffaeebf..f9aa666e30 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupVelocityControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + 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( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..3078359028 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -23,6 +23,7 @@ #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; @@ -58,6 +59,7 @@ class JointGroupVelocityControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ From 96a8d5798c1e2a80b104d2fb3312e1444dc142c5 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 16 Jul 2024 06:50:22 -0600 Subject: [PATCH 19/27] Unused header cleanup (#1199) --- .../test/test_ackermann_steering_controller.cpp | 2 -- .../test/test_ackermann_steering_controller.hpp | 8 ++------ ...st_ackermann_steering_controller_preceeding.cpp | 2 -- .../admittance_controller.hpp | 8 -------- .../admittance_controller/admittance_rule.hpp | 13 +++---------- .../admittance_controller/admittance_rule_impl.hpp | 5 +++-- .../src/admittance_controller.cpp | 4 ---- .../test/test_admittance_controller.cpp | 2 -- .../test/test_admittance_controller.hpp | 7 +++---- .../test/test_asset_6d_robot_description.hpp | 2 -- .../test/test_bicycle_steering_controller.cpp | 2 -- .../test/test_bicycle_steering_controller.hpp | 8 ++------ ...test_bicycle_steering_controller_preceeding.cpp | 2 -- .../diff_drive_controller.hpp | 4 ---- .../test/test_diff_drive_controller.cpp | 2 -- .../joint_group_effort_controller.hpp | 3 --- .../src/joint_group_effort_controller.cpp | 1 - .../test/test_joint_group_effort_controller.cpp | 5 ----- .../test/test_joint_group_effort_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 3 --- .../test/test_force_torque_sensor_broadcaster.cpp | 5 ----- .../test/test_force_torque_sensor_broadcaster.hpp | 4 ++-- .../forward_command_controller.hpp | 2 -- .../forward_controllers_base.hpp | 1 - .../multi_interface_forward_command_controller.hpp | 2 -- .../src/forward_command_controller.cpp | 3 --- .../src/forward_controllers_base.cpp | 2 -- .../hardware_interface_adapter.hpp | 1 - .../test/test_gripper_controllers.cpp | 4 ---- .../test/test_gripper_controllers.hpp | 4 ++-- .../imu_sensor_broadcaster.hpp | 3 --- .../test/test_imu_sensor_broadcaster.cpp | 5 ----- .../test/test_imu_sensor_broadcaster.hpp | 4 ++-- .../joint_state_broadcaster.hpp | 2 -- .../src/joint_state_broadcaster.cpp | 7 +------ .../test/test_joint_state_broadcaster.cpp | 5 +---- .../test/test_joint_state_broadcaster.hpp | 4 ++-- .../joint_trajectory_controller.hpp | 2 -- .../joint_trajectory_controller/tolerances.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 3 --- joint_trajectory_controller/src/trajectory.cpp | 1 - joint_trajectory_controller/test/test_assets.hpp | 2 -- .../test/test_load_joint_trajectory_controller.cpp | 4 ++-- .../test/test_tolerances.cpp | 5 ++--- .../test/test_trajectory.cpp | 7 ++----- .../test/test_trajectory_actions.cpp | 8 -------- .../test/test_trajectory_controller.cpp | 14 +------------- .../include/pid_controller/pid_controller.hpp | 6 ------ pid_controller/src/pid_controller.cpp | 3 --- pid_controller/test/test_pid_controller.cpp | 1 - pid_controller/test/test_pid_controller.hpp | 7 ++----- .../test/test_pid_controller_preceding.cpp | 2 -- .../joint_group_position_controller.hpp | 3 --- .../src/joint_group_position_controller.cpp | 1 - .../test/test_joint_group_position_controller.cpp | 5 ----- .../test/test_joint_group_position_controller.hpp | 4 ++-- .../range_sensor_broadcaster.hpp | 3 --- .../steering_controllers_library.hpp | 6 ------ .../steering_odometry.hpp | 3 +-- .../src/steering_controllers_library.cpp | 4 ---- .../src/steering_odometry.cpp | 1 - .../test/test_steering_controllers_library.cpp | 3 --- .../test/test_steering_controllers_library.hpp | 8 ++------ .../test/test_steering_odometry.cpp | 2 +- .../include/tricycle_controller/odometry.hpp | 2 +- .../tricycle_controller/tricycle_controller.hpp | 3 --- .../test/test_tricycle_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.hpp | 8 ++------ ...est_tricycle_steering_controller_preceeding.cpp | 2 -- .../joint_group_velocity_controller.hpp | 3 --- .../src/joint_group_velocity_controller.cpp | 1 - .../test/test_joint_group_velocity_controller.cpp | 5 ----- .../test/test_joint_group_velocity_controller.hpp | 4 ++-- 74 files changed, 45 insertions(+), 243 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index de45accc0a..c04e87be95 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 9379d956c9..320a1a91a6 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "ackermann_steering_controller/ackermann_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 1a16bed838..96dd20d80e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 6ff6cdae7a..9be6c3298c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,7 +17,6 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include #include #include #include @@ -29,21 +28,14 @@ #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" -#include "trajectory_msgs/msg/joint_trajectory.hpp" - namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 36c027491c..7223dbe9d1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -18,23 +18,16 @@ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #include -#include -#include + #include #include #include +#include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" -#include "control_toolbox/filters.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "controller_interface/controller_interface_base.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" -#include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_kdl/tf2_kdl.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9b03924882..77f1277b35 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,9 +22,10 @@ #include #include +#include +#include + #include "rclcpp/duration.hpp" -#include "rclcpp/utilities.hpp" -#include "tf2_ros/transform_listener.h" namespace admittance_controller { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..6e29b574a2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,17 +16,13 @@ #include "admittance_controller/admittance_controller.hpp" -#include #include -#include #include #include #include #include "admittance_controller/admittance_rule_impl.hpp" #include "geometry_msgs/msg/wrench.hpp" -#include "rcutils/logging_macros.h" -#include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 256fe1e5fe..5c4bbe9438 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -16,9 +16,7 @@ #include "test_admittance_controller.hpp" -#include #include -#include #include // Test on_init returns ERROR when a required parameter is missing diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index a464d050be..d2aef3df4b 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -17,6 +17,8 @@ #ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ #define TEST_ADMITTANCE_CONTROLLER_HPP_ +#include + #include #include #include @@ -25,21 +27,18 @@ #include #include -#include "gmock/gmock.h" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/parameter_value.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" #include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" -#include "trajectory_msgs/msg/joint_trajectory.hpp" // TODO(anyone): replace the state and command message types using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; diff --git a/admittance_controller/test/test_asset_6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp index 4d38df7c30..11412ebbab 100644 --- a/admittance_controller/test/test_asset_6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ -#include - namespace ros2_control_test_assets { const auto valid_6d_robot_urdf = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index a06aabbd20..573992b24e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 4643ee3ee9..a6b3f8ae40 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "bicycle_steering_controller/bicycle_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index bc3a182753..0bc03f4886 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest 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 72b38f7d2d..ac34b81eca 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 @@ -30,15 +30,11 @@ #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4976da4dfa..bc664f0711 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -26,7 +25,6 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 562bbea52f..b07e8a630c 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -15,11 +15,8 @@ #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#include - #include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace effort_controllers { diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index c3748d493c..e7d0f274a5 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -17,7 +17,6 @@ #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/logging.hpp" #include "rclcpp/parameter.hpp" namespace effort_controllers diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 06bbc6fd22..c19cbff9cf 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index e871ac6c43..5af880d792 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 754d1de9ba..bd477ed68a 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,15 +20,12 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 9528adbb0f..2872e8a60f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -22,14 +22,9 @@ #include #include -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; using testing::IsEmpty; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe5b0ab3ba..fe7fb0c174 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 91e3aae480..12f2a28b22 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index d60245f328..4858e5ab64 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,6 @@ #include "controller_interface/controller_interface.hpp" #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "std_msgs/msg/float64_multi_array.hpp" diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index fd7c0d480e..50476c62e3 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0305a37e4e..78fe8c9191 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,14 +14,11 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include #include #include -#include #include #include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..331d3f9eea 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,10 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" -#include #include #include -#include #include #include "controller_interface/helpers.hpp" diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index b125ab12d0..2cc24794b8 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 9f7e024917..97fef24eaf 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -23,9 +22,6 @@ #include "test_gripper_controllers.hpp" #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 4983c8102d..cb92bc9dcd 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -15,12 +15,12 @@ #ifndef TEST_GRIPPER_CONTROLLERS_HPP_ #define TEST_GRIPPER_CONTROLLERS_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "gripper_controllers/gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index cd2a44d32b..1ba0b032ac 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -20,14 +20,11 @@ #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/imu_sensor.hpp" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index e8aa571112..a2da6713bb 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,12 +23,7 @@ #include #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..0f3286c302 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_ #define TEST_IMU_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables 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 f1c532dce9..b3fa69f94c 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 @@ -25,8 +25,6 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a53fe2b3c4..5eb0a9b9b6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -14,21 +14,16 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" -#include +#include #include #include #include #include #include -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcpputils/split.hpp" -#include "rcutils/logging_macros.h" #include "std_msgs/msg/header.hpp" namespace rclcpp_lifecycle diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 4e4c0631f0..e479f344bf 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,12 +23,9 @@ #include "gmock/gmock.h" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fa9d29c936..3e54116d5c 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_STATE_BROADCASTER_HPP_ #define TEST_JOINT_STATE_BROADCASTER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" 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 c3fbb58456..e22dee8946 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 @@ -15,7 +15,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ -#include #include // for std::reference_wrapper #include #include @@ -36,7 +35,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c4404920df..8c556703ab 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -38,9 +38,6 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/time.hpp" - namespace joint_trajectory_controller { /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..ddcf57f508 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -22,10 +22,7 @@ #include #include "angles/angles.h" -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "controller_interface/helpers.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..54f785a070 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -20,7 +20,6 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp index ccdbaf4c8a..df6f2a37c7 100644 --- a/joint_trajectory_controller/test/test_assets.hpp +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSETS_HPP_ #define TEST_ASSETS_HPP_ -#include - namespace test_trajectory_controllers { const auto urdf_rrrbot_revolute = diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 5b36afc4e8..8d0fbb3e75 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -#include "gmock/gmock.h" +#include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index bd6304df29..106a9a761f 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include -#include #include -#include "gmock/gmock.h" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6e0c53ac77..b0d7ad8e18 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include #include #include -#include "gmock/gmock.h" - -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3e65016403..164f7d0e11 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -15,38 +15,30 @@ #ifndef _MSC_VER #include #endif -#include #include #include #include #include -#include #include #include -#include #include #include #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" -#include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" -#include "hardware_interface/resource_manager.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "test_trajectory_controller_utils.hpp" using std::placeholders::_1; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21c5705505..e83e74ef41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -12,35 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -#include #include -#include -#include #include -#include #include #include #include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f7b8cc4491..236b067929 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -28,17 +28,11 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "control_msgs/msg/joint_controller_state.hpp" - -#include "control_msgs/msg/pid_state.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - namespace pid_controller { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b76926d5a0..c8e6cc0fe0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -24,9 +24,6 @@ #include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" -#include "controller_interface/helpers.hpp" - -#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" namespace diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 4e9601ae66..9b53dccb23 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 8c36dca60f..895c56c1a5 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -18,20 +18,17 @@ #ifndef TEST_PID_CONTROLLER_HPP_ #define TEST_PID_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "pid_controller/pid_controller.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 3e17e69286..498ca633da 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -17,10 +17,8 @@ #include "test_pid_controller.hpp" -#include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 47705b6a3d..4eaf9086e4 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -15,11 +15,8 @@ #ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" #include "position_controllers/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace position_controllers { diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 8335a200c4..1074ba7ef7 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -17,7 +17,6 @@ #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/logging.hpp" #include "rclcpp/parameter.hpp" namespace position_controllers diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a4a288a8e4..96a5cead17 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index c7d704db52..fefd2073e3 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index b2e5fbfac0..5a93f95982 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -20,13 +20,10 @@ #define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/range_sensor.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e417b9fcd1..84a892d79e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -15,21 +15,16 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ -#include #include #include -#include #include -#include #include #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/set_bool.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" @@ -37,7 +32,6 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" 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 dadc5730d4..5b67797b79 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -22,8 +22,7 @@ #include #include -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 1aef556212..f193098f82 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -16,15 +16,11 @@ #include #include -#include #include #include #include -#include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index f8b42771e2..ba431faf33 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -20,7 +20,6 @@ #include "steering_controllers_library/steering_odometry.hpp" #include -#include #include namespace steering_odometry diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 98ab97fdc3..fca7d00946 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -17,11 +17,8 @@ #include #include #include -#include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" - class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index ae7c41a09b..b257562dcd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -15,21 +15,17 @@ #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d93e29eca5..e3c8db6c15 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#include #include "steering_controllers_library/steering_odometry.hpp" diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 13d65a980a..f3252df12f 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -21,7 +21,7 @@ #include -#include "rclcpp/time.hpp" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index b6f9aae417..010a890f52 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -31,12 +31,9 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 901597a8bc..e56e3afacb 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -29,7 +28,6 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" #include "tricycle_controller/tricycle_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index ad7f80607f..3f2589cb6c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 8a3ea33925..184439aa6f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -15,21 +15,17 @@ #ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 6f2913aeb8..2170659ee7 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index b5c36f433a..e443ba76cc 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -15,10 +15,7 @@ #ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "velocity_controllers/visibility_control.h" namespace velocity_controllers diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index eb55e052dc..97f6713d72 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -16,7 +16,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index f9aa666e30..d781b6ca5c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 3078359028..0ebe68833c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" From f7b1af1841c42bf6c1dc617b170e5abc0150c99c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 11:40:05 +0200 Subject: [PATCH 20/27] Change the subscription timeout in the tests to 5ms (#1219) --- .../test/test_ackermann_steering_controller.hpp | 2 +- admittance_controller/test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_force_torque_sensor_broadcaster.cpp | 2 +- imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 +- joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 4 ++-- pid_controller/test/test_pid_controller.hpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 320a1a91a6..b0b0944de2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -216,7 +216,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d2aef3df4b..b2a95c12fa 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -270,7 +270,7 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node_->get_clock()->now() + timeout; while (!received_msg && test_subscription_node_->get_clock()->now() < until) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index a6b3f8ae40..bfcf9fa424 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -190,7 +190,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2872e8a60f..1acddcb64a 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -85,7 +85,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index a2da6713bb..132bf4006e 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -86,7 +86,7 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index e479f344bf..71e65a978d 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -677,7 +677,7 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (!received_msg && test_node.get_clock()->now() < until) { @@ -755,7 +755,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (test_node.get_clock()->now() < until) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 895c56c1a5..326393bac0 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -186,7 +186,7 @@ class PidControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ef9d5187a3..e8be5e3d0a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -80,7 +80,7 @@ void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Ran while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b257562dcd..8c4955b7eb 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -237,7 +237,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 184439aa6f..0fbea25abf 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -203,7 +203,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { From 990bfb645425c166a923ddb53ddd98ec66ea42d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 18 Jul 2024 14:47:05 +0200 Subject: [PATCH 21/27] Remove duplicated call to rclcpp::shutdown in test (#1220) --- diff_drive_controller/test/test_load_diff_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index bcd334631f..aa23f83ec9 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -41,6 +41,5 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); int result = RUN_ALL_TESTS(); rclcpp::shutdown(); - rclcpp::shutdown(); return result; } From a95364449b110056c3f3d389e1a1903cc23996ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jul 2024 11:03:44 +0200 Subject: [PATCH 22/27] Use the internal methods instead of using the variables directly (#1221) --- .../src/joint_trajectory_controller.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ddcf57f508..faa2efce9b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,9 +43,10 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { - if (!urdf_.empty()) + const std::string & urdf = get_robot_description(); + if (!urdf.empty()) { - if (!model_.initString(urdf_)) + if (!model_.initString(urdf)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); } @@ -701,7 +702,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { - if (!urdf_.empty()) + if (!get_robot_description().empty()) { auto urdf_joint = model_.getJoint(params_.joints[i]); if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) From abadffae4f8d4420e3cd36e52d502ed7d09b418c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 21 Jul 2024 19:28:44 +0200 Subject: [PATCH 23/27] Add missing includes (#1226) --- .../test/test_ackermann_steering_controller.hpp | 2 ++ .../test/test_bicycle_steering_controller.hpp | 2 ++ diff_drive_controller/test/test_diff_drive_controller.cpp | 2 ++ .../test/test_force_torque_sensor_broadcaster.cpp | 2 ++ imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 ++ joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 2 ++ pid_controller/test/test_pid_controller.hpp | 2 ++ range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp | 2 ++ .../test/test_steering_controllers_library.hpp | 2 ++ tricycle_controller/test/test_tricycle_controller.cpp | 2 ++ .../test/test_tricycle_steering_controller.hpp | 2 ++ 11 files changed, 22 insertions(+) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index b0b0944de2..59637a072f 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -26,6 +26,8 @@ #include "ackermann_steering_controller/ackermann_steering_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index bfcf9fa424..65f1691a3b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -26,6 +26,8 @@ #include "bicycle_steering_controller/bicycle_steering_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index bc664f0711..72ae4dbfd7 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1acddcb64a..1ea25520cc 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -24,6 +24,8 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" using hardware_interface::LoanedStateInterface; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 132bf4006e..67cf31f8a5 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,6 +23,8 @@ #include #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "sensor_msgs/msg/imu.hpp" diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 71e65a978d..7400d6bbb9 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "test_joint_state_broadcaster.hpp" diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 326393bac0..158b5d9147 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -29,6 +29,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "pid_controller/pid_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index e8be5e3d0a..59d27ebc0c 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -21,6 +21,8 @@ #include "test_range_sensor_broadcaster.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using testing::IsEmpty; using testing::SizeIs; diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 8c4955b7eb..93ee823e0f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index e56e3afacb..9d43c2590d 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -28,6 +28,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "tricycle_controller/tricycle_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 0fbea25abf..3b7a053937 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" From 609b86763822e3c1276643de0b1f4f2aeb90ce90 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jul 2024 23:28:34 +0200 Subject: [PATCH 24/27] [JTC] Refactor URDF Model parsing (#1227) --- joint_trajectory_controller/CMakeLists.txt | 4 ++ .../joint_trajectory_controller.hpp | 3 - .../src/joint_trajectory_controller.cpp | 63 +++++++++---------- .../test/test_trajectory_controller.cpp | 7 +++ .../test/test_trajectory_controller_utils.hpp | 8 ++- 5 files changed, 47 insertions(+), 38 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 46c245be9c..438661f7bf 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -59,10 +59,12 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) + ament_target_dependencies(test_trajectory ros2_control_test_assets) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_tolerances test/test_tolerances.cpp) target_link_libraries(test_tolerances joint_trajectory_controller) + ament_target_dependencies(test_tolerances ros2_control_test_assets) target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_trajectory_controller @@ -71,6 +73,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_controller joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_controller ros2_control_test_assets) target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_load_joint_trajectory_controller @@ -93,6 +96,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_actions ros2_control_test_assets) endif() 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 e22dee8946..2f0a1f4df0 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 @@ -41,7 +41,6 @@ #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "urdf/model.h" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" @@ -300,8 +299,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); - urdf::Model model_; - /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index faa2efce9b..9563568ad5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,15 +43,44 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + const std::string & urdf = get_robot_description(); if (!urdf.empty()) { - if (!model_.initString(urdf)) + urdf::Model model; + if (!model.initString(urdf)) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse robot description!"); + return CallbackReturn::ERROR; } else { + /// initialize the URDF model and update the joint angles wraparound vector + // Configure joint position error normalization (angle_wraparound) + joints_angle_wraparound_.resize(params_.joints.size(), false); + for (size_t i = 0; i < params_.joints.size(); ++i) + { + auto urdf_joint = model.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); } } @@ -61,18 +90,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); } - try - { - // Create the parameter listener and get the parameters - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; } @@ -698,24 +715,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization (angle_wraparound) - joints_angle_wraparound_.resize(dof_); - for (size_t i = 0; i < dof_; ++i) - { - if (!get_robot_description().empty()) - { - auto urdf_joint = model_.getJoint(params_.joints[i]); - if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - RCLCPP_DEBUG( - logger, "joint '%s' is of type continuous, use angle_wraparound.", - params_.joints[i].c_str()); - joints_angle_wraparound_[i] = true; - } - // do nothing if joint is not found in the URDF - } - } - if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e83e74ef41..f188c0f04b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -39,6 +39,13 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; +TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) +{ + ASSERT_EQ( + controller_interface::return_type::ERROR, + SetUpTrajectoryControllerLocal({}, "")); +} + TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a15b2e5bc5..796503c036 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "ros2_control_test_assets/descriptions.hpp" namespace { @@ -249,7 +250,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) @@ -260,7 +261,8 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}, const std::string & urdf = "") + const std::vector & parameters = {}, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { traj_controller_ = std::make_shared(); @@ -302,7 +304,7 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto has_nonzero_vel_param = std::find_if( From adcb88e8fa2d98b82f2dd7dcd8b690c9c0cb1085 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:27:35 +0100 Subject: [PATCH 25/27] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 9 + admittance_controller/CHANGELOG.rst | 8 + bicycle_steering_controller/CHANGELOG.rst | 9 + diff_drive_controller/CHANGELOG.rst | 9 + effort_controllers/CHANGELOG.rst | 7 + force_torque_sensor_broadcaster/CHANGELOG.rst | 9 + forward_command_controller/CHANGELOG.rst | 7 + gripper_controllers/CHANGELOG.rst | 7 + imu_sensor_broadcaster/CHANGELOG.rst | 9 + joint_state_broadcaster/CHANGELOG.rst | 9 + joint_trajectory_controller/CHANGELOG.rst | 10 + parallel_gripper_controller/CHANGELOG.rst | 196 ++++++++++++++++++ parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 9 + position_controllers/CHANGELOG.rst | 7 + range_sensor_broadcaster/CHANGELOG.rst | 9 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 8 + tricycle_controller/CHANGELOG.rst | 8 + tricycle_steering_controller/CHANGELOG.rst | 9 + velocity_controllers/CHANGELOG.rst | 7 + 23 files changed, 356 insertions(+), 1 deletion(-) create mode 100644 parallel_gripper_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 21840061d4..7ecc861d1a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 4ae5ea39f8..b13ca47d31 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* 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 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 831aa5a2b3..dc98a4cfe7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 00f777df67..9a57056434 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Remove duplicated call to rclcpp::shutdown in test (`#1220 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Noel Jiménez García, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 70cfaad021..d6e77f5151 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index eac8c9959a..eff0763f9b 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 17196fed65..08bdeca044 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 968a1088c2..fcfd574be7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Henry Moore, Paul Gesel, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5bacde755e..e0d338fdae 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cc01373289..f4e5891353 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 047d232155..7ad799d30b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Refactor URDF Model parsing (`#1227 `_) +* Use the internal methods instead of using the variables directly (`#1221 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* [JTC] Fix test_tolerances_via_actions (`#1209 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst new file mode 100644 index 0000000000..2e828ac6f8 --- /dev/null +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -0,0 +1,196 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package parallel_gripper_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* [GripperController] Fix failing tests (`#1210 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Paul Gesel, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- + +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) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +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) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +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) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +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) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index f189071617..b180e07fc5 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 - 2.32.0 + 4.11.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 8e45f042f0..9736cdf480 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c9e2c749b5..873982845b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 31458c07d6..b427c11c7d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a54d39b85d..b1c335bd26 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 78e18dffaa..be2c3c0e35 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 +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 20450f3ca3..55af17d40b 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index d4f62b806c..7ccb978487 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * Fix steering controllers library kinematics (`#1150 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index fdde4481e2..869801a7bc 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6f4ad5d8c1..79e0a71af0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 614e023984..f7f4103cef 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) From e4b7827c7545c03aa977bbde92149606cb73e6a9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:27:54 +0100 Subject: [PATCH 26/27] 4.12.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 +- 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 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/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 +- 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 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7ecc861d1a..8ce11f3a9e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index dfa733515f..62e9a5a27b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.11.0 + 4.12.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b13ca47d31..390eae4878 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d6e13b0634..2e056c12a7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.11.0 + 4.12.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index dc98a4cfe7..ee689b77ae 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 7afcf53706..37a552cd7d 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.11.0 + 4.12.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 9a57056434..6fcf3e272c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Remove duplicated call to rclcpp::shutdown in test (`#1220 `_) * Unused header cleanup (`#1199 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d7fa06b762..a819df01e0 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.11.0 + 4.12.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d6e77f5151..0f4271ba91 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index a7744927a1..6f3438c70e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index eff0763f9b..134faec535 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 ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 90fcb3a684..063239659b 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.11.0 + 4.12.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 08bdeca044..bfabe442ac 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f4c02d8de7..7ff4e85139 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fcfd574be7..3334033fa9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix parallel gripper controller CI (`#1202 `_) * Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 72d2ec48f2..a61a650c4d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.11.0 + 4.12.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e0d338fdae..d73046d7e3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5f0e981605..5c3d3b1196 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.11.0 + 4.12.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f4e5891353..8ec080072f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 77bbe2ca00..a6bee4983a 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.11.0 + 4.12.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 7ad799d30b..043f71623e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * [JTC] Refactor URDF Model parsing (`#1227 `_) * Use the internal methods instead of using the variables directly (`#1221 `_) * Unused header cleanup (`#1199 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index d32d4e3daf..3da60aa2c5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.11.0 + 4.12.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 2e828ac6f8..56f5b21426 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * [GripperController] Fix failing tests (`#1210 `_) * Fix parallel gripper controller CI (`#1202 `_) * Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index b180e07fc5..2bceb44069 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 - 4.11.0 + 4.12.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 9736cdf480..c7a23dcba0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 2a44c9ad1b..335f48f91c 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.11.0 + 4.12.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 873982845b..95bba79c9d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 33f0f0c15e..b3a3fc6ce8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b427c11c7d..762ba806ba 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index e4525f53fd..8ad0290ff7 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.11.0 + 4.12.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b1c335bd26..52d97ab7d2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 24e4429ade..a01eb0921e 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.11.0 + 4.12.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index be2c3c0e35..2cc1bb37dd 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 ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 87af6e6695..12388322bc 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.11.0 + 4.12.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 91db299739..394c11dab5 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.11.0", + version="4.12.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 55af17d40b..d6e70f3a47 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 ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 85c44c58fe..4f5674760d 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 - 4.11.0 + 4.12.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 2588a47cc8..60d3944d61 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.11.0", + version="4.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 7ccb978487..04f10c3158 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 7d172d45a8..3d300ae22e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.11.0 + 4.12.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 869801a7bc..890bdbb026 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 20310e6b9a..e7d8c3438a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.11.0 + 4.12.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 79e0a71af0..6966dce4dc 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 4aeb8a086f..50340a466b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.11.0 + 4.12.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f7f4103cef..9d52d6fd25 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d128f2b45b..8a3385b76e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 33e35f0cf330d51290c13405e5af04544c795fe5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nikola=20Banovi=C4=87?= Date: Wed, 31 Jul 2024 20:04:46 +0200 Subject: [PATCH 27/27] Fix admittance controller interface read/write logic (#1232) --- admittance_controller/src/admittance_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e29b574a2..a4b56d739c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -476,13 +476,13 @@ void AdmittanceController::read_state_from_hardware( state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); nan_position |= std::isnan(state_current.positions[joint_ind]); } - else if (has_velocity_state_interface_) + if (has_velocity_state_interface_) { state_current.velocities[joint_ind] = state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); nan_velocity |= std::isnan(state_current.velocities[joint_ind]); } - else if (has_acceleration_state_interface_) + if (has_acceleration_state_interface_) { state_current.accelerations[joint_ind] = state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); @@ -528,15 +528,15 @@ void AdmittanceController::write_state_to_hardware( command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } - else if (has_velocity_command_interface_) + if (has_velocity_command_interface_) { command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.velocities[joint_ind]); } - else if (has_acceleration_command_interface_) + if (has_acceleration_command_interface_) { command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.accelerations[joint_ind]); } } last_commanded_ = state_commanded;