From b9cc2600a02f464c8eca9331b7c3206879d0c336 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 19 Jun 2024 18:08:53 +0100 Subject: [PATCH 1/8] Still fill desired/actual deprecated fields (#1172) --- .../src/joint_trajectory_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7705f7870c..f23cd970fb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1180,6 +1180,12 @@ void JointTrajectoryController::publish_state( state_publisher_->msg_.reference.velocities = desired_state.velocities; state_publisher_->msg_.reference.accelerations = desired_state.accelerations; state_publisher_->msg_.feedback.positions = current_state.positions; + // DESIRED and ACTUAL are deprecated in the message but we are still + // reporting on them + state_publisher_legacy_->msg_.desired.positions = desired_state.positions; + state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities; + state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_legacy_->msg_.actual.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) { From 8c991912ca07f5d12f1172b36fa2df7a0cea7097 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 19 Jun 2024 22:27:55 +0200 Subject: [PATCH 2/8] [STEERING] Add missing `tan` call for ackermann (#1117) (#1176) (cherry picked from commit b81be48fb1b6101a3c601c0ea11c5393e5a7026e) Co-authored-by: Enrique Llorente Pastora --- 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 e663eddf91..63331f3c17 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 17ca070adffb6abd35d142b661ab669323354cfc Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 19 Jun 2024 22:28:41 +0200 Subject: [PATCH 3/8] [Steering controllers library] Reference interfaces are body twist (#1168) (#1173) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit (cherry picked from commit 615524831a3bd3e0fc4e4678c6eb9afd7f39f326) Co-authored-by: Christoph Fröhlich --- .../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 0352a79cab..c28e692b78 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -301,7 +301,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 b82509440b..0c894087be 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -265,7 +265,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 f1bb2d0681..b455752bbc 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 095db7674b..8e57e10ea0 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 26207d54b7..01e3d1b796 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -370,7 +370,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; @@ -447,7 +447,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 8a0d21cc6c..edc4575176 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -323,7 +323,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 c2528e1fc9..278994a4f3 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -284,7 +284,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 5d8a1f57ab2ca9600ec4c65c5f78a272274a2b96 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 1 Jul 2024 14:31:46 +0200 Subject: [PATCH 4/8] Bump version of pre-commit hooks (#1185) (#1186) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> (cherry picked from commit dc928985b34c8bf439bc92f210d26209d09f262c) Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@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 0f3dd37f9ecfd79a944c2f8cce8e8170d8766249 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 10:33:52 +0100 Subject: [PATCH 5/8] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ 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 | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 9 +++++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 20 files changed, 86 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7ea692784d..14aa241b89 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- * Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 566af67641..6eb974a41a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 6b411e436b..ce4bfa195e 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- * Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index fc6082dec9..d35d5a7b44 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- * Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 9ed16a20eb..9b04057560 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 95b355943b..bf106260ff 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 +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 950754cf82..5f0689f59d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fa6256068e..954e22708c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index bc3804d43f..40ad80d615 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 00c7e28617..435b178a75 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c867c65193..533fed5693 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Still fill desired/actual deprecated fields (`#1172 `_) +* JTC trajectory end time validation fix (`#1090 `_) (`#1140 `_) +* Contributors: Bence Magyar, mergify[bot] + 2.35.0 (2024-05-22) ------------------- * [JTC] Remove unused test code (`#1095 `_) (`#1096 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 434a39a202..f7e7fdd257 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index bd74424f2e..a3e08cf3a7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4e47e9f5d1..fe1cc833c4 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) (`#1142 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 9e475377f1..98d6f18c2c 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 +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index af29863bdf..e8b7106edf 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 +----------- + 2.35.0 (2024-05-22) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 61102a93ff..31cb0fe850 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) (`#1176 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) (`#1153 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 11f4026066..825c79782d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) +* Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a74f9b9bac..1a528cac47 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) +* Contributors: mergify[bot] + 2.35.0 (2024-05-22) ------------------- * Add parameter check for geometric values (backport `#1120 `_) (`#1125 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8b57fea87c..4dd212a4ac 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.35.0 (2024-05-22) ------------------- From 0766dac7a7760c47726fe291cdaaec6d32e58894 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 10:34:09 +0100 Subject: [PATCH 6/8] 2.36.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 +- 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 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 14aa241b89..1c7a0f9ab9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) * Contributors: mergify[bot] diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 013f813276..62d5fcd8a0 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 2.35.0 + 2.36.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 6eb974a41a..13465aef00 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 842b8c25aa..0ce3bf694a 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.35.0 + 2.36.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 ce4bfa195e..7a39d43fb6 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) * Contributors: mergify[bot] diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 5ed4cf4048..e03f942db6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 2.35.0 + 2.36.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 d35d5a7b44..1d4d4d56c8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) * Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) * Contributors: mergify[bot] diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9e1faa353d..779e3bb056 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.35.0 + 2.36.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 9b04057560..35c3910ccd 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index fe211c18f6..4dbc6f63f0 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.35.0 + 2.36.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 bf106260ff..0c16c8c21f 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 ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 9a6f0a960f..888dff4fd5 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.35.0 + 2.36.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 5f0689f59d..d259f13282 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index e431589b63..e792309177 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.35.0 + 2.36.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 954e22708c..0d36535d43 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index e69d1ada4a..a2e6af3f3f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.35.0 + 2.36.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 40ad80d615..98bdd88f76 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index cbfb5b3e48..010df65dbb 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.35.0 + 2.36.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 435b178a75..d413a18cf4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 057b6bd5d7..1cb19b6e4d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.35.0 + 2.36.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 533fed5693..26773559b5 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * Still fill desired/actual deprecated fields (`#1172 `_) * JTC trajectory end time validation fix (`#1090 `_) (`#1140 `_) * Contributors: Bence Magyar, mergify[bot] diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b8492ee59a..6660317ae3 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.35.0 + 2.36.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index f7e7fdd257..8db257d9e4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index af303b8b2d..b4421bde70 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.35.0 + 2.36.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 a3e08cf3a7..cc11770dbe 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 006b2e9ed1..e18f77381b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 2.35.0 + 2.36.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 fe1cc833c4..c41aeff163 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) (`#1142 `_) * Contributors: mergify[bot] diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 686f6190de..054bae8d4a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.35.0 + 2.36.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 98d6f18c2c..c4f3dad622 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 ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index e6c2b0803f..476d8bd436 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.35.0 + 2.36.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 e394f9a5c6..427e8fd66c 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.35.0", + version="2.36.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 e8b7106edf..967d35c0ec 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 ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 5f1ab877cd..c7ce8cc8b1 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 - 2.35.0 + 2.36.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 91f4b2eb82..7d421abe96 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="2.35.0", + version="2.36.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 31cb0fe850..947eff7df4 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) (`#1176 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 8fd152596e..48f03a974e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 2.35.0 + 2.36.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 825c79782d..a2cb5e7989 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) (`#1160 `_) * Bump version of pre-commit hooks (`#1157 `_) (`#1158 `_) * Contributors: mergify[bot] diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 1e7f596958..2064b95146 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.35.0 + 2.36.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 1a528cac47..28cd68749c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) (`#1173 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) (`#1164 `_) * Contributors: mergify[bot] diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index b34cb737d3..173c259e18 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 2.35.0 + 2.36.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 4dd212a4ac..d78dfe246a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.36.0 (2024-07-09) +------------------- 2.35.0 (2024-05-22) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 6ba013685c..25d86a2fc7 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.35.0 + 2.36.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 112df35d9b1fd4353035a9bbac1c1a256b7eec15 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 15 Jul 2024 18:31:24 +0100 Subject: [PATCH 7/8] [JTC] Process tolerances sent with action goal (backport #716) (#1189) --- doc/controllers_index.rst | 4 +- doc/migration.rst | 13 + doc/release_notes.rst | 53 +++ 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 +- 11 files changed, 974 insertions(+), 36 deletions(-) create mode 100644 doc/migration.rst create mode 100644 doc/release_notes.rst create mode 100644 joint_trajectory_controller/test/test_tolerances.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index bff6db552d..cfda4d8195 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 new file mode 100644 index 0000000000..2c35f57aee --- /dev/null +++ b/doc/migration.rst @@ -0,0 +1,13 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst + +Migration Guides: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes important changes between Galactic (previous) and Humble (current) releases, where changes to user code might be necessary. + +.. note:: + + This list was created in July 2024, earlier changes are not included. + +joint_trajectory_controller +***************************** + * 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.rst b/doc/release_notes.rst new file mode 100644 index 0000000000..4e720ada81 --- /dev/null +++ b/doc/release_notes.rst @@ -0,0 +1,53 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst + +Release Notes: Galactic to Humble +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes the changes between Galactic (previous) and Humble (current) releases. Bugfixes are not included in this list. + +.. note:: + + This list was created in July 2024, earlier changes may not be included. + +diff_drive_controller +***************************** +* Remove unused parameter ``wheels_per_side`` (`#958 `_). + +joint_trajectory_controller +***************************** + +* Activate update of dynamic parameters (`#761 `_ and `#849 `_). +* Continue with last trajectory-point on success, instead of hold-position from current state (`#842 `_). +* Add console output for tolerance checks (`#932 `_): + + .. code:: + + [tolerances]: State tolerances failed for joint 2: + [tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000 + [trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds + +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* 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 `_). +* 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 +************************ +* 🚀 The PID controller was added 🎉 (`#434 `_). + +steering_controllers_library +******************************** +* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 0f06d5d3ea..9b5b1396bf 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -60,6 +60,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 8b7fcc3041..9042bf6e62 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -153,7 +153,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 a0b080de35..0d58375295 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 @@ -233,7 +233,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 1681e7e037..204cd531f2 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 f23cd970fb..d14a5d24c2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -121,11 +121,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_) @@ -235,6 +236,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 @@ -242,7 +244,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()); @@ -259,8 +261,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; } @@ -268,14 +269,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 @@ -355,7 +356,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()); @@ -374,7 +375,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()); @@ -393,7 +394,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()); @@ -403,7 +404,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()); @@ -411,7 +412,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()); @@ -655,11 +656,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; } @@ -816,6 +817,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); @@ -942,6 +945,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(); @@ -949,7 +954,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) @@ -961,8 +966,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; } } @@ -975,8 +980,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; } } @@ -1019,9 +1024,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; } } @@ -1290,6 +1294,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 d49f522cff..d200008f7b 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 @@ -129,6 +163,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 143bfc963a8704a5be14435ab12d48945603a571 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 15 Jul 2024 18:52:23 +0100 Subject: [PATCH 8/8] Fix steering controllers library kinematics (#1150) (#1194) --- .../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 b0488a7d4a..471c8231fc 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 @@ -31,6 +32,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) @@ -183,10 +187,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 @@ -225,6 +230,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 01e3d1b796..9e6ccdf1f9 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -452,7 +452,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 63331f3c17..9ccf73f1c7 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(