From 50036e109d6a03e41bb9b43a4f67411887b48640 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 25 Sep 2024 22:53:41 +0200 Subject: [PATCH 01/36] fix(steering-odometry): convert twist to steering angle (#1288) --- steering_controllers_library/src/steering_odometry.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 824ec86f59..dbe210ed41 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -208,12 +208,9 @@ 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 (fabs(v_bx) < std::numeric_limits::epsilon()) - { - // avoid division by zero - return 0.; - } - return std::atan(omega_bz * wheelbase_ / v_bx); + // phi can be nan if both v_bx and omega_bz are zero + const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + return std::isfinite(phi) ? phi : 0.0; } std::tuple, std::vector> SteeringOdometry::get_commands( From aee0f8256d241b412fc42b4814d76fc8e38a8342 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 09:38:31 +0100 Subject: [PATCH 02/36] Bump version of pre-commit hooks (#1300) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 43bb778260..63e7f08682 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.0 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +109,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.29.3 hooks: - id: check-github-workflows args: ["--verbose"] From 10040541b9cca8e83408315d6747f3b0fcf7cf27 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 1 Oct 2024 10:39:09 +0200 Subject: [PATCH 03/36] fix(timeout): do not reset steer wheels to 0. on timeout (#1289) --- doc/release_notes.rst | 1 + .../src/steering_controllers_library.cpp | 29 ++++++------------ .../test_steering_controllers_library.cpp | 30 +++++++++++-------- 3 files changed, 28 insertions(+), 32 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b55db861b3..1fb66c4475 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -56,6 +56,7 @@ 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 `_). * A fix for Ackermann steering odometry was added (`#921 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..836574f150 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -355,26 +355,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } return controller_interface::return_type::OK; @@ -396,13 +381,17 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = 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++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -414,7 +403,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..e73f32cd5e 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -142,10 +142,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -174,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -183,10 +186,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); } int main(int argc, char ** argv) From 9c5be9f44ee0880b2d99b393bc5b3b49f95a8cc7 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 7 Oct 2024 20:14:25 +0200 Subject: [PATCH 04/36] Adapt test to new way of exporting reference interfaces (Related to #1240 in ros2_control) (#1103) --------- Co-authored-by: Bence Magyar --- .../test/test_ackermann_steering_controller.cpp | 6 +++--- ...test_ackermann_steering_controller_preceeding.cpp | 6 +++--- .../test/test_bicycle_steering_controller.cpp | 6 +++--- .../test_bicycle_steering_controller_preceeding.cpp | 6 +++--- pid_controller/test/test_pid_controller.cpp | 6 +++--- .../test/test_pid_controller_preceding.cpp | 12 ++++++------ .../test/test_steering_controllers_library.cpp | 6 +++--- .../test/test_tricycle_steering_controller.cpp | 6 +++--- .../test_tricycle_steering_controller_preceeding.cpp | 6 +++--- 9 files changed, 30 insertions(+), 30 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c04e87be95..c434f73467 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -89,9 +89,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 96dd20d80e..4f67beaa07 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -91,9 +91,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 573992b24e..7ec0983f93 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -73,9 +73,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0bc03f4886..0a321009c2 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -77,9 +77,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9b53dccb23..60d484e463 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -97,9 +97,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 9e6a7ef04c..ee5fe46754 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -84,9 +84,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } @@ -101,10 +101,10 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string state_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(exported_state_itfs[esi_index].get_name(), state_itf_name); + EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); EXPECT_EQ( - exported_state_itfs[esi_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(exported_state_itfs[esi_index].get_interface_name(), dof_name + "/" + interface); + exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); ++esi_index; } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index e73f32cd5e..3378efbef8 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -70,9 +70,9 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 3f2589cb6c..8e29314f8e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -81,9 +81,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 2170659ee7..566169e34e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -84,9 +84,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } From 5e1ecbf51bf45f5e03f37c0eaafd7daed0bc66e5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:24:58 +0100 Subject: [PATCH 05/36] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 3 +++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 7 +++++++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 22 files changed, 78 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 066ef29830..71d615dbc3 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index bb640c49fe..ae4e47bcad 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index d2a06f3329..ec4523f9c9 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4907b259bf..95b11d2c40 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 388fde8829..61ee0921d1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 30c2e36143..6d8ea46c1f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04a2dd86c5..dc496e5566 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fc2e5c571a..c03d284e02 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 89a39358ed..26613db617 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 21dfc85d82..4db42c39e4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8b35a6986c..0979f0c073 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index f8e7c77ddb..bd99385f23 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 08e37d0836..0aa8677522 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- * [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 17db75c10b..b645154cdf 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 36335db719..16e164a2f4 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 70fdee135d..e93bcf4403 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 5f2afedd10..ab1debdaef 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * Fix deprecation warning in paramater declaration (`#1280 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 4cd8b487d8..c6c414caf4 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * Fix bug for displaying all controllers (`#1259 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ab57a3b720..c2aeee8fef 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) +* fix(steering-odometry): convert twist to steering angle (`#1288 `_) +* Contributors: Manuel Muth, Rein Appeldoorn + 4.14.0 (2024-09-11) ------------------- * fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 82721a5d1a..f27568de08 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 697878ac81..6b12ac5c44 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 5a662df92b..089267d835 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- From 97c1e2472f2bbc98a9cb86449b41611a3e298b30 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:25:30 +0100 Subject: [PATCH 06/36] 4.15.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 71d615dbc3..bc2997b8a9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 3c9836e97b..5764de56c1 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.14.0 + 4.15.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 ae4e47bcad..46899f9cd6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d9d6d24b7e..06fde3cb59 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.14.0 + 4.15.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 ec4523f9c9..0d096951ca 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 5858b53dc3..f58cf42dff 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.14.0 + 4.15.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 95b11d2c40..4c48fb234d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 7f4cb4cbd6..45ab80d5cb 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.14.0 + 4.15.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 61ee0921d1..ea2efdcf4e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 1208cd4732..fab6d671ad 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.14.0 + 4.15.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 6d8ea46c1f..7d2a7a1a8a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 758f448fdf..1383342246 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.14.0 + 4.15.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 dc496e5566..54caa9d764 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index bfa8a68955..f9f30dd053 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.14.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c03d284e02..11c61f86cf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 019f1e2e44..d46c98f608 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.14.0 + 4.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 26613db617..40af7a5360 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 0ed268d9ad..ed1f794afa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.14.0 + 4.15.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 4db42c39e4..cf422e3eb8 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 8776cb31d3..c3a1e66574 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.14.0 + 4.15.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 0979f0c073..81696246b1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index c8b670dac4..112c4ad9a1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.14.0 + 4.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index bd99385f23..a3024fbf62 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 055b5311b7..b396ba7b85 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.14.0 + 4.15.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 0aa8677522..5a27471199 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 9ef1964cd3..79a66d3cc3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.14.0 + 4.15.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b645154cdf..d52201f2b4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e79dd05059..c03888eaf8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.14.0 + 4.15.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 16e164a2f4..77eae62d2a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 433120ca8d..92283fe3ad 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.14.0 + 4.15.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 e93bcf4403..bd9e0a8b38 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6ff486a1d8..0886b489c7 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.14.0 + 4.15.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 ab1debdaef..29ca6cd84d 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0b30bdd7ad..f61a51e34e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.14.0 + 4.15.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 071e90ca0b..59aee66875 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.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 c6c414caf4..043930c536 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c5973fd96e..7920e63246 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.14.0 + 4.15.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 6c390b22e9..a70342e154 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.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 c2aeee8fef..39360d6921 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) * fix(steering-odometry): convert twist to steering angle (`#1288 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 41f7e3703d..5c96c4624c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.14.0 + 4.15.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 f27568de08..34520eeb83 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0fea2baab6..4e6ab510f3 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.14.0 + 4.15.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 6b12ac5c44..4e1f9abd1e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 5bf5a2d283..27aac82dd1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.14.0 + 4.15.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 089267d835..a04bf1c4ae 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e7ebdcc827..ea295db0ed 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.14.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f96d2fc0fbf94537f769cffcf844858f7a085671 Mon Sep 17 00:00:00 2001 From: Kenta Kato Date: Sat, 26 Oct 2024 23:57:38 +0900 Subject: [PATCH 07/36] [JTC] Add Parameter to Toggle State Setting on Activation (#1231) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [JTC] Add param to setting last command interface value as state on activation * [JTC] add a note about set_last_command_interface_value_as_state_on_activation to release_notes. Updated the parameters.yaml description to match the same wording. --------- Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- doc/release_notes.rst | 2 ++ .../src/joint_trajectory_controller.cpp | 4 +++- .../src/joint_trajectory_controller_parameters.yaml | 5 +++++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 1fb66c4475..688c35724d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -48,6 +48,8 @@ joint_trajectory_controller * -1 - The tolerance is "erased". If there was a default, the joint will be allowed to move without restriction. +* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e4923604fd..bea37e02be 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -924,7 +924,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if ( + params_.set_last_command_interface_value_as_state_on_activation && + read_state_from_command_interfaces(state)) { state_current_ = state; last_commanded_state_ = state; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 8bd64b6314..14b71f0711 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -64,6 +64,11 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } + set_last_command_interface_value_as_state_on_activation: { + type: bool, + default_value: true, + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + } action_monitor_rate: { type: double, default_value: 20.0, From 7c89c17a5cf6ac5d553c79ccc63182f2e1454388 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Sun, 27 Oct 2024 00:03:25 +0200 Subject: [PATCH 08/36] Implement new PoseBroadcaster controller (#1311) --- doc/controllers_index.rst | 1 + pose_broadcaster/CMakeLists.txt | 106 ++++++++++ pose_broadcaster/README.md | 8 + pose_broadcaster/doc/userdoc.rst | 27 +++ .../pose_broadcaster/pose_broadcaster.hpp | 77 +++++++ .../pose_broadcaster/visibility_control.h | 49 +++++ pose_broadcaster/package.xml | 30 +++ pose_broadcaster/pose_broadcaster.xml | 9 + pose_broadcaster/src/pose_broadcaster.cpp | 195 +++++++++++++++++ .../src/pose_broadcaster_parameters.yaml | 32 +++ .../test/pose_broadcaster_params.yaml | 4 + .../test/test_load_pose_broadcaster.cpp | 50 +++++ .../test/test_pose_broadcaster.cpp | 198 ++++++++++++++++++ .../test/test_pose_broadcaster.hpp | 96 +++++++++ 14 files changed, 882 insertions(+) create mode 100644 pose_broadcaster/CMakeLists.txt create mode 100644 pose_broadcaster/README.md create mode 100644 pose_broadcaster/doc/userdoc.rst create mode 100644 pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp create mode 100644 pose_broadcaster/include/pose_broadcaster/visibility_control.h create mode 100644 pose_broadcaster/package.xml create mode 100644 pose_broadcaster/pose_broadcaster.xml create mode 100644 pose_broadcaster/src/pose_broadcaster.cpp create mode 100644 pose_broadcaster/src/pose_broadcaster_parameters.yaml create mode 100644 pose_broadcaster/test/pose_broadcaster_params.yaml create mode 100644 pose_broadcaster/test/test_load_pose_broadcaster.cpp create mode 100644 pose_broadcaster/test/test_pose_broadcaster.cpp create mode 100644 pose_broadcaster/test/test_pose_broadcaster.hpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d5b956aa8a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -71,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> Common Controller Parameters diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..46028cf258 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) +ament_target_dependencies(pose_broadcaster PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + pose_broadcaster_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0ae40e2fad --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..621a90cc85 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "pose_broadcaster_parameters.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr pose_sensor_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; + + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..eae2ccad2c --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,30 @@ + + + + pose_broadcaster + 0.0.0 + Broadcaster to publish cartesian states. + Robert Wilbrandt + + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..6578958004 --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..7e3aeaddf3 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 "pose_broadcaster/pose_broadcaster.hpp" + +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + +namespace pose_broadcaster +{ + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; + + try + { + pose_publisher_ = get_node()->create_publisher( + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + + if (params_.tf.enable) + { + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize pose message + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } + + realtime_tf_publisher_->unlock(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose = pose; + realtime_publisher_->unlockAndPublish(); + } + + if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..11c53b5e57 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null + tf: + enable: + type: bool + default_value: true + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..a2f8477dd1 --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_pose_broadcaster: + ros__parameters: + pose_name: "test_pose" + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..bdf72d7b23 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,50 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm{ + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"}; + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..0ed2e84619 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,198 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 "test_pose_broadcaster.hpp" + +#include +#include + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init( + "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..a164b2c6ac --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef TEST_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; + + std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + template + void subscribe_and_get_message(const std::string & topic, T & msg); +}; + +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + +#endif // TEST_POSE_BROADCASTER_HPP_ From fa42b5ec97b0af5420060844b7027b8e8912c05d Mon Sep 17 00:00:00 2001 From: Gilmar Correia Date: Thu, 31 Oct 2024 08:46:39 -0300 Subject: [PATCH 09/36] fixes for windows compilation (#1330) Co-authored-by: SENAI-GilmarCorreia --- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- pid_controller/CMakeLists.txt | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8c556703ab..4902fd1dcc 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -133,7 +133,7 @@ double resolve_tolerance_source(const double default_value, const double goal_va // * -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) + auto is_erase_value = [=](double value) { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; if (goal_value > 0.0) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 6c9e00ef8b..f9a9abce89 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() +if(WIN32) + add_compile_definitions( + # For math constants + _USE_MATH_DEFINES + # Minimize Windows namespace collision + NOMINMAX + WIN32_LEAN_AND_MEAN + ) +endif() + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs From feaf122bcb15113d3c446e03bb1df6ff2e3ba6aa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 1 Nov 2024 09:49:37 +0100 Subject: [PATCH 10/36] Bump version of pre-commit hooks (#1334) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63e7f08682..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.0 + rev: v19.1.3 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.29.3 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] From 1716351d0a9b8f3cef9281095d9a72d5621643c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Nov 2024 11:34:01 +0100 Subject: [PATCH 11/36] Add hardware_interface_testing dependency (#1335) --- pose_broadcaster/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index eae2ccad2c..4c3506f1cd 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 87f21b39c5e81327eb8adf8ed16e5fde1101bd97 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 1 Nov 2024 20:13:05 +0100 Subject: [PATCH 12/36] [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (#1331) * Add test to reproduce the behaviour of https://github.com/ros-controls/ros2_control_demos/pull/417#discussion_r1823443110 * Add fix to solve the issue of the publishing non-existing joint_states --- .../src/joint_state_broadcaster.cpp | 3 + .../test/test_joint_state_broadcaster.cpp | 96 +++++++++++++++++++ .../test/test_joint_state_broadcaster.hpp | 1 + 3 files changed, 100 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index fe0b32213a..9bbb862925 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -209,6 +209,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_.clear(); + name_if_value_mapping_.clear(); return CallbackReturn::SUCCESS; } @@ -309,6 +310,8 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.joint_names.clear(); + dynamic_joint_state_msg.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 877d199419..de534c00b6 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -216,6 +216,102 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + + SetUpStateBroadcaster(); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of + // the interface is unavailable) + ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + const std::vector JOINT_NAMES = {"joint1", "joint2"}; + assign_state_interfaces(JOINT_NAMES, interface_names_); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size(); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + + // dynamic joint state initialized + const auto & new_dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) { const std::vector JOINT_NAMES = {}; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 0b4c50e89e..4f4241d3d7 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -34,6 +34,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); + FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); From 4343c7a4632a97f52fc664b5dcdcad258ffb5e2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 12:50:08 +0100 Subject: [PATCH 13/36] [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (#1215) --- .../force_torque_sensor_broadcaster.xml | 2 +- .../force_torque_sensor_broadcaster.hpp | 16 +++- .../src/force_torque_sensor_broadcaster.cpp | 80 ++++++++++++++++++- ..._torque_sensor_broadcaster_parameters.yaml | 33 ++++++++ .../test_force_torque_sensor_broadcaster.cpp | 78 ++++++++++++++++++ 5 files changed, 202 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml index 10d19a93c5..8e61e013de 100644 --- a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml +++ b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml @@ -1,6 +1,6 @@ + type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface"> This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message. diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index bd477ed68a..e5a5349c32 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,8 +20,9 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,7 +33,7 @@ namespace force_torque_sensor_broadcaster { -class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface +class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC @@ -59,10 +60,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update( + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + std::vector on_export_state_interfaces() override; + protected: + void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + std::shared_ptr param_listener_; Params params_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9b570d353f..ae105a511c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -24,7 +24,7 @@ namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() -: controller_interface::ControllerInterface() +: controller_interface::ChainableControllerInterface() { } @@ -141,23 +141,97 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update( +controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + } if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); + this->apply_sensor_offset(params_, realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; } +controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +std::vector +ForceTorqueSensorBroadcaster::on_export_state_interfaces() +{ + std::vector exported_state_interfaces; + + std::vector force_names( + {params_.interface_names.force.x, params_.interface_names.force.y, + params_.interface_names.force.z}); + std::vector torque_names( + {params_.interface_names.torque.x, params_.interface_names.torque.y, + params_.interface_names.torque.z}); + if (!params_.sensor_name.empty()) + { + const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); + std::copy( + semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); + std::copy( + semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + } + const std::string controller_name = get_node()->get_name(); + if (!force_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + } + if (!force_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + } + if (!force_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + } + if (!torque_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + } + if (!torque_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + } + if (!torque_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + } + return exported_state_interfaces; +} + +void ForceTorqueSensorBroadcaster::apply_sensor_offset( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x += params.offset.force.x; + msg.wrench.force.y += params.offset.force.y; + msg.wrench.force.z += params.offset.force.z; + msg.wrench.torque.x += params.offset.torque.x; + msg.wrench.torque.y += params.offset.torque.y; + msg.wrench.torque.z += params.offset.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 3e75ab6012..0869f5cf3c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -46,3 +46,36 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + offset: + force: + x: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'z' axis.", + } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1ea25520cc..e436beb2e5 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -278,6 +278,60 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) +{ + SetUpFTSBroadcaster(); + + std::array force_offsets = {10.0, 30.0, -50.0}; + std::array torque_offsets = {1.0, -1.2, -5.2}; + // set the params 'sensor_name' and 'frame_id' + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i]->get_value(), + sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); @@ -300,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x)); ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y)); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 2u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -328,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + } } int main(int argc, char ** argv) From f2da66240082c1abf2e25a624640b148171e861d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Nov 2024 16:50:59 +0100 Subject: [PATCH 14/36] Update ros2_controllers.humble.repos (#1350) --- ros2_controllers.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 2a3b96551d..a0432f65b3 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -10,7 +10,7 @@ repositories: kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: master + version: humble control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git From 9439764015ef4a8b245ed07381459c19883e5a2b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 6 Nov 2024 22:47:17 +0100 Subject: [PATCH 15/36] Adding use of robot description parameter in the Admittance Controller (#1247) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Kevin DeMarco Co-authored-by: Nikola Banovic Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- .../include/admittance_controller/admittance_rule.hpp | 3 ++- .../admittance_controller/admittance_rule_impl.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 4 +++- .../test/test_admittance_controller.hpp | 9 +++------ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..a326b663d0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -102,7 +102,8 @@ class AdmittanceRule /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, const size_t num_joint); + const std::shared_ptr & node, const size_t num_joint, + const std::string & robot_description); /// Reset all values back to default controller_interface::return_type reset(const size_t num_joints); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index cab8b4cf45..13d4e67fbc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/admittance_rule.hpp" #include +#include #include #include @@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( - const std::shared_ptr & node, const size_t num_joints) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -58,7 +60,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, node->get_node_parameters_interface(), "kinematics")) { return controller_interface::return_type::ERROR; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..6e0e38140a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -280,7 +280,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, this->get_robot_description()) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..4f4635d861 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -102,7 +102,6 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon } } -private: const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -110,10 +109,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon class AdmittanceControllerTest : public ::testing::Test { public: - static void SetUpTestCase() - { - // rclcpp::init(0, nullptr); - } + static void SetUpTestCase() {} void SetUp() { @@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", 0, "", options); + auto result = + controller_->init(controller_name, controller_->robot_description_, 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); From b0391e2069c8ec9a94ce35cb3e395bac3ca62eb6 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Thu, 7 Nov 2024 09:06:38 +0100 Subject: [PATCH 16/36] [jtc] Improve trajectory sampling efficiency (#1297) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- .../trajectory.hpp | 21 +++- .../src/trajectory.cpp | 5 +- .../test/test_trajectory.cpp | 115 ++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..14373b006e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,6 +69,9 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * + * This function assumes that sampling is only done at monotonically increasing \p sample_time + * for any trajectory. + * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() @@ -85,9 +88,12 @@ class Trajectory * * \param[in] sample_time Time at which trajectory will be sampled. * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at - * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr - * Iterator to the start segment for given \p sample_time. See description above. \param[out] - * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * all. + * \param[out] output_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See + * description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See + * description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -147,6 +153,14 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } + /// Get the index of the segment start returned by the last \p sample() operation. + /** + * As the trajectory is only accessed at monotonically increasing sampling times, this index is + * used to speed up the selection of relevant trajectory points. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + size_t last_sample_index() const { return last_sample_idx_; } + private: void deduce_from_derivatives( trajectory_msgs::msg::JointTrajectoryPoint & first_state, @@ -160,6 +174,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + size_t last_sample_idx_ = 0; }; /** diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 54f785a070..512b16f3c5 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -84,6 +84,7 @@ void Trajectory::update(std::shared_ptr j trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; + last_sample_idx_ = 0; } bool Trajectory::sample( @@ -149,7 +150,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + for (size_t i = last_sample_idx_; i < last_idx; ++i) { auto & point = trajectory_msg_->points[i]; auto & next_point = trajectory_msg_->points[i + 1]; @@ -175,6 +176,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + last_sample_idx_ = i; return true; } } @@ -182,6 +184,7 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); + last_sample_idx_ = last_idx; output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b0d7ad8e18..44f511aa31 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -44,10 +44,12 @@ TEST(TestTrajectory, initialize_trajectory) empty_msg->header.stamp.nanosec = 2; const rclcpp::Time empty_time = empty_msg->header.stamp; auto traj = joint_trajectory_controller::Trajectory(empty_msg); + EXPECT_EQ(0, traj.last_sample_index()); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -64,6 +66,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -109,6 +112,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -121,6 +125,7 @@ TEST(TestTrajectory, sample_trajectory_positions) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -129,6 +134,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -142,6 +148,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -154,6 +161,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -165,6 +173,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -174,6 +183,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -182,6 +192,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -193,6 +204,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -350,6 +362,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.last_sample_index(), 0); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -362,6 +375,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -370,6 +384,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -390,6 +405,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -404,6 +420,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -424,6 +441,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -438,6 +456,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -458,6 +477,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -471,6 +491,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -523,6 +544,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -536,6 +558,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -544,6 +567,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -564,6 +588,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -615,6 +640,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -628,6 +654,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -642,6 +669,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -658,6 +686,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -674,6 +703,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -686,6 +716,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -732,6 +763,7 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { traj.sample(time_now, no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -747,6 +779,7 @@ TEST(TestTrajectory, skip_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -755,6 +788,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -771,6 +805,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -786,6 +821,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -796,6 +832,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -804,6 +841,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -812,6 +850,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -819,6 +858,82 @@ TEST(TestTrajectory, skip_interpolation) } } +TEST(TestTrajectory, update_trajectory) +{ + // Verify that sampling works correctly after updating with a new trajectory + auto first_msg = std::make_shared(); + first_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + first_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + first_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + first_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, first_msg); + EXPECT_EQ(0, traj.last_sample_index()); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Sample at starting time + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + + // Sample 2.5s after msg + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(1, traj.last_sample_index()); + + // Update trajectory + auto snd_msg = std::make_shared(); + snd_msg->header.stamp = rclcpp::Time(0); + + snd_msg->points.push_back(p1); + snd_msg->points.push_back(p2); + snd_msg->points.push_back(p3); + + traj.update(snd_msg); + + // Sample at starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR( + (p1.positions[0] - point_before_msg.positions[0]), expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(std::next(traj.begin()), end); + EXPECT_NEAR((p1.positions[0] + p2.positions[0]) / 2, expected_state.positions[0], EPS); + } +} + TEST(TestWrapAroundJoint, no_wraparound) { const std::vector initial_position(3, 0.); From 7ed1a0ee49187139bcf2b574b0b7c992fe0d06a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:43:45 +0100 Subject: [PATCH 17/36] [JTC] Fix the JTC length_error exceptions in the tests (#1360) --- .../test/test_trajectory_controller_utils.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 796503c036..a850891331 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" namespace @@ -391,6 +392,21 @@ class TrajectoryControllerTest : public ::testing::Test return traj_controller_->get_node()->activate(); } + void DeactivateTrajectoryController() + { + if (traj_controller_) + { + if ( + traj_controller_->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + EXPECT_EQ( + traj_controller_->get_node()->deactivate().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + } + } + static void TearDownTestCase() { rclcpp::shutdown(); } void subscribeToState(rclcpp::Executor & executor) @@ -776,6 +792,8 @@ class TrajectoryControllerTestParameterized state_interface_types_ = std::get<1>(GetParam()); } + virtual void TearDown() { TrajectoryControllerTest::DeactivateTrajectoryController(); } + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; From 55ed331f761113be5f3acf3018ffde2f78872f6c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:32 +0000 Subject: [PATCH 18/36] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 5 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 3 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 3 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 5 + joint_trajectory_controller/CHANGELOG.rst | 8 + parallel_gripper_controller/CHANGELOG.rst | 3 + pid_controller/CHANGELOG.rst | 5 + pose_broadcaster/CHANGELOG.rst | 210 ++++++++++++++++++ pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 24 files changed, 290 insertions(+), 1 deletion(-) create mode 100644 pose_broadcaster/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index bc2997b8a9..8f13e3cb6f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 46899f9cd6..bff4354f4a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding use of robot description parameter in the Admittance Controller (`#1247 `_) +* Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich + 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 0d096951ca..82a4e610bb 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4c48fb234d..e806e497af 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ea2efdcf4e..2cc63d7c90 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7d2a7a1a8a..258b1d52a3 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54caa9d764..8130b880b3 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 11c61f86cf..5feab361b1 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 40af7a5360..214b914e08 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cf422e3eb8..47ce981e63 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 81696246b1..e873080f17 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) +* [jtc] Improve trajectory sampling efficiency (`#1297 `_) +* fixes for windows compilation (`#1330 `_) +* [JTC] Add Parameter to Toggle State Setting on Activation (`#1231 `_) +* Contributors: Gilmar Correia, Kenta Kato, RobertWilbrandt, Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index a3024fbf62..da2098ecd0 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 5a27471199..c4ce2430f6 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fixes for windows compilation (`#1330 `_) +* Contributors: Gilmar Correia + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..729df5a2f9 --- /dev/null +++ b/pose_broadcaster/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pose_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add hardware_interface_testing dependency (`#1335 `_) +* Implement new PoseBroadcaster controller (`#1311 `_) +* Contributors: Christoph Fröhlich, RobertWilbrandt + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 4c3506f1cd..6a71e11c7e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 0.0.0 + 4.15.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d52201f2b4..eb6d8df132 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77eae62d2a..c485b49efe 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bd9e0a8b38..51c989d4f2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 29ca6cd84d..7a9ab97dc6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 043930c536..5db2db7bad 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 39360d6921..44fed2a58c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 34520eeb83..c867767b9a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4e1f9abd1e..3542beded0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index a04bf1c4ae..82ceab063a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- From f519170c776649de11431addbfe5bba84ac3603d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:52 +0000 Subject: [PATCH 19/36] 4.16.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- 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 +- 48 files changed, 71 insertions(+), 71 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 8f13e3cb6f..1f275bd7ba 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 5764de56c1..128eabae4c 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.15.0 + 4.16.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 bff4354f4a..0d7daa5f5d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Adding use of robot description parameter in the Admittance Controller (`#1247 `_) * Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 06fde3cb59..1a08bff889 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.15.0 + 4.16.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 82a4e610bb..57d23a977a 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index f58cf42dff..c87e4e6642 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.15.0 + 4.16.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 e806e497af..e79f1faf1d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 45ab80d5cb..84f51a0750 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.15.0 + 4.16.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 2cc63d7c90..0abc5feee9 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index fab6d671ad..36ab9ce79e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.15.0 + 4.16.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 258b1d52a3..6bd9e2febd 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1383342246..237cb2c888 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.15.0 + 4.16.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 8130b880b3..ae920d9f83 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f9f30dd053..8a02785a3a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5feab361b1..291cab691a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d46c98f608..76877ff6fa 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.15.0 + 4.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 214b914e08..2219d53fe4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index ed1f794afa..9de0f9c316 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.15.0 + 4.16.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 47ce981e63..ed2f4b31eb 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c3a1e66574..9e5412881c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.15.0 + 4.16.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 e873080f17..87089f700a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) * [jtc] Improve trajectory sampling efficiency (`#1297 `_) * fixes for windows compilation (`#1330 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 112c4ad9a1..c21db502ec 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.15.0 + 4.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index da2098ecd0..6ee5646ac4 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index b396ba7b85..5fbb69303e 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.15.0 + 4.16.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c4ce2430f6..9c20af4e97 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * fixes for windows compilation (`#1330 `_) * Contributors: Gilmar Correia diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 79a66d3cc3..0a22ecfe7b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.15.0 + 4.16.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 729df5a2f9..c12bc9da80 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Add hardware_interface_testing dependency (`#1335 `_) * Implement new PoseBroadcaster controller (`#1311 `_) * Contributors: Christoph Fröhlich, RobertWilbrandt diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 6a71e11c7e..01c91f21f2 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.15.0 + 4.16.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index eb6d8df132..e5478f518a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c03888eaf8..17bf0b736e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.15.0 + 4.16.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 c485b49efe..5d97df0e0b 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 92283fe3ad..3b4276951f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.15.0 + 4.16.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 51c989d4f2..a45a425f24 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0886b489c7..6eb4144647 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.15.0 + 4.16.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 7a9ab97dc6..d3c3059abf 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f61a51e34e..1ad38581a2 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.15.0 + 4.16.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 59aee66875..fb26e9c5f7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.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 5db2db7bad..555a636d7e 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 7920e63246..5954bcd03d 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.15.0 + 4.16.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 a70342e154..f2ee71c3d7 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.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 44fed2a58c..417c4c9f05 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5c96c4624c..93d304637d 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.15.0 + 4.16.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 c867767b9a..31258cac7c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4e6ab510f3..568bd5cb46 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.15.0 + 4.16.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 3542beded0..975c48303d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 27aac82dd1..66c5456b9b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.15.0 + 4.16.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 82ceab063a..90b143f675 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ea295db0ed..60ca9c693c 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From de77fd0fad6cd85ab23027c1b4d685b2f8085526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:48:15 +0100 Subject: [PATCH 20/36] Update maintainers and add url tags (#1363) --- ackermann_steering_controller/package.xml | 13 ++++++++++--- admittance_controller/package.xml | 15 +++++++++++++-- bicycle_steering_controller/package.xml | 13 ++++++++++--- diff_drive_controller/package.xml | 16 ++++++++++++++-- effort_controllers/package.xml | 11 ++++++++++- force_torque_sensor_broadcaster/package.xml | 14 ++++++++++++-- forward_command_controller/package.xml | 11 ++++++++++- gripper_controllers/package.xml | 9 +++++++++ imu_sensor_broadcaster/package.xml | 13 +++++++++++-- joint_state_broadcaster/package.xml | 14 +++++++++++--- joint_trajectory_controller/package.xml | 8 +++++++- parallel_gripper_controller/package.xml | 9 +++++++++ pid_controller/package.xml | 13 +++++++++++-- pose_broadcaster/package.xml | 12 +++++++++++- position_controllers/package.xml | 11 ++++++++++- range_sensor_broadcaster/package.xml | 14 ++++++++++++-- ros2_controllers/package.xml | 9 +++++++-- ros2_controllers_test_nodes/package.xml | 10 ++++++++-- rqt_joint_trajectory_controller/package.xml | 9 ++++++++- steering_controllers_library/package.xml | 14 ++++++++++---- tricycle_controller/package.xml | 11 ++++++++++- tricycle_steering_controller/package.xml | 14 ++++++++++---- velocity_controllers/package.xml | 11 ++++++++++- 23 files changed, 233 insertions(+), 41 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 128eabae4c..6ae2f0a438 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -4,10 +4,17 @@ ackermann_steering_controller 4.16.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 - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 1a08bff889..5b48c69695 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -4,11 +4,22 @@ admittance_controller 4.16.0 Implementation of admittance controllers for different input and output interface. - Denis Štogl + Bence Magyar - Andy Zelenak + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Andy Zelenak + Paul Gesel + ament_cmake backward_ros diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c87e4e6642..3fafbda077 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -4,10 +4,17 @@ bicycle_steering_controller 4.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 84f51a0750..0602d91898 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -2,12 +2,24 @@ diff_drive_controller 4.16.0 - Controller for a differential drive mobile base. + Controller for a differential-drive mobile base. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Bence Magyar + Enrique Fernández + Manuel Meraz + Jordan Palacios + ament_cmake generate_parameter_library diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 36ab9ce79e..b33224db9b 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -3,11 +3,20 @@ effort_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 237cb2c888..f2f5124001 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -4,11 +4,21 @@ force_torque_sensor_broadcaster 4.16.0 Controller to publish state of force-torque sensors. + Bence Magyar - Denis Štogl - Subhas Das + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Subhas Das + ament_cmake backward_ros diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8a02785a3a..be71c32052 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -3,11 +3,20 @@ forward_command_controller 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 76877ff6fa..33f06f226a 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -6,11 +6,20 @@ gripper_controllers 4.16.0 The gripper_controllers package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Jafar Abdi ament_cmake diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 9de0f9c316..a09f09dee2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -4,11 +4,20 @@ imu_sensor_broadcaster 4.16.0 Controller to publish readings of IMU sensors. + Bence Magyar - Denis Štogl - Victor Lopez + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Victor Lopez + ament_cmake backward_ros diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9e5412881c..575c17c5a0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -3,13 +3,21 @@ joint_state_broadcaster 4.16.0 Broadcaster to publish joint state + Bence Magyar - Denis Stogl - Jordan Palacios - Karsten Knese + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + Karsten Knese + ament_cmake backward_ros diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index c21db502ec..830321e7b6 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -3,12 +3,18 @@ joint_trajectory_controller 4.16.0 Controller for executing joint-space trajectories on a group of joints + Bence Magyar - Dr. Denis Štogl + Denis Štogl Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + ament_cmake angles diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 5fbb69303e..7e712b632a 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -6,11 +6,20 @@ parallel_gripper_controller 4.16.0 The parallel_gripper_controller package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Paul Gesel ament_cmake diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 0a22ecfe7b..fb3f40184a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -4,10 +4,19 @@ pid_controller 4.16.0 Controller based on PID implememenation from control_toolbox package. + Bence Magyar - Denis Štogl + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Denis Štogl - Apache-2.0 ament_cmake diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 01c91f21f2..dc5d629b50 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -4,10 +4,20 @@ pose_broadcaster 4.16.0 Broadcaster to publish cartesian states. - Robert Wilbrandt + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Robert Wilbrandt + ament_cmake backward_ros diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 17bf0b736e..8aa7bf8117 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -3,11 +3,20 @@ position_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3b4276951f..a81823a6a2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -3,11 +3,21 @@ range_sensor_broadcaster 4.16.0 - Controller to publish readings of Range sensors. + Controller to publish readings of range sensors. + Bence Magyar - Florent Chretien + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Florent Chretien + ament_cmake backward_ros diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6eb4144647..ccde6cf812 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -2,13 +2,18 @@ ros2_controllers 4.16.0 - Metapackage for ROS2 controllers related packages + Metapackage for ros2_controllers related packages + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ ament_cmake diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 1ad38581a2..c53f1b9971 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -5,10 +5,16 @@ 4.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. - Denis Štogl Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 - Apache-2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ rclpy std_msgs diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 5954bcd03d..da92d9d663 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -8,8 +8,15 @@ Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota - Apache-2.0 + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 93d304637d..efe9f93a67 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -4,11 +4,17 @@ steering_controllers_library 4.16.0 Package for steering robot configurations including odometry and interfaces. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 568bd5cb46..516adef248 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -4,9 +4,18 @@ tricycle_controller 4.16.0 Controller for a tricycle drive mobile base + Bence Magyar - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Tony Najjar ament_cmake diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 66c5456b9b..7c26e09ba0 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -4,11 +4,17 @@ tricycle_steering_controller 4.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 60ca9c693c..50e662cc58 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,11 +3,20 @@ velocity_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros From 28dd7e12987bad86e131c0f2c5684f8c84a61c59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:53:32 +0100 Subject: [PATCH 21/36] TractionLimiter: Fix wrong input checks (#1341) --- tricycle_controller/CMakeLists.txt | 6 + .../tricycle_controller/traction_limiter.hpp | 15 +- tricycle_controller/src/traction_limiter.cpp | 30 +- .../test/test_traction_limiter.cpp | 546 ++++++++++++++++++ 4 files changed, 590 insertions(+), 7 deletions(-) create mode 100644 tricycle_controller/test/test_traction_limiter.cpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 6834b753a0..7afe150c43 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -85,6 +85,12 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_traction_limiter + test/test_traction_limiter.cpp) + target_link_libraries(test_traction_limiter + tricycle_controller + ) endif() install( diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..437f6cd4c2 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -28,14 +28,25 @@ class TractionLimiter public: /** * \brief Constructor + * + * Parameters are applied symmetrically for both directions, i.e., are applied + * to the absolute value of the corresponding quantity. + * + * \warning + * - Setting min_velocity: the robot can't stand still + * + * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity + * + * - Setting min_jerk: the robot can't move with constant acceleration + * * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] - * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3] + * \param [in] max_jerk Maximum jerk [m/s^3] */ TractionLimiter( double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index a8019714e1..e31e5e1e82 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -44,13 +44,13 @@ TractionLimiter::TractionLimiter( if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; - if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0; if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; const std::string error = - "The positive limit will be applied to both directions. Setting different limits for positive " + " The positive limit will be applied to both directions. Setting different limits for positive " "and negative directions is not supported. Actuators are " "assumed to have the same constraints in both directions"; if (min_velocity_ < 0 || max_velocity_ < 0) @@ -58,19 +58,39 @@ TractionLimiter::TractionLimiter( throw std::invalid_argument("Velocity cannot be negative." + error); } + if (min_velocity_ > max_velocity_) + { + throw std::invalid_argument("Min velocity cannot be greater than max velocity."); + } + if (min_acceleration_ < 0 || max_acceleration_ < 0) { - throw std::invalid_argument("Acceleration cannot be negative." + error); + throw std::invalid_argument("Acceleration limits cannot be negative." + error); + } + + if (min_acceleration_ > max_acceleration_) + { + throw std::invalid_argument("Min acceleration cannot be greater than max acceleration."); } if (min_deceleration_ < 0 || max_deceleration_ < 0) { - throw std::invalid_argument("Deceleration cannot be negative." + error); + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) + { + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } if (min_jerk_ < 0 || max_jerk_ < 0) { - throw std::invalid_argument("Jerk cannot be negative." + error); + throw std::invalid_argument("Jerk limits cannot be negative." + error); + } + + if (min_jerk_ > max_jerk_) + { + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } diff --git a/tricycle_controller/test/test_traction_limiter.cpp b/tricycle_controller/test/test_traction_limiter.cpp new file mode 100644 index 0000000000..4afed7f217 --- /dev/null +++ b/tricycle_controller/test/test_traction_limiter.cpp @@ -0,0 +1,546 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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 "tricycle_controller/traction_limiter.hpp" + +TEST(SpeedLimiterTest, testWrongParams) +{ + EXPECT_NO_THROW(tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + )); + + // velocity + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + -20., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + -10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + 20., // min_velocity + 10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // acceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + -20., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + -10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + 20., // min_acceleration + 10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // deceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + -20., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + -10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + 20., // min_deceleration + 10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // jerk + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + -20. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + -10. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + 20., // min_jerk + 10. // max_jerk + ), + std::invalid_argument); + } +} + +TEST(SpeedLimiterTest, testNoLimits) +{ + tricycle_controller::TractionLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(SpeedLimiterTest, testVelocityLimits) +{ + tricycle_controller::TractionLimiter limiter(0.5, 1.0, 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + { + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 1.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + + v = 0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 0.1); + + // TODO(christophfroehlich): does this behavior make sense? + v = 0.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -1.0); + EXPECT_DOUBLE_EQ(limiting_factor, -1.0 / -10.0); + + v = -0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, -0.5 / -0.1); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testAccelerationLimits) +{ + { + // test max_acceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + { + // test min_acceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testDecelerationLimits) +{ + { + // test max_deceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit_acceleration(v, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } + { + // test min_deceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 9.9; + limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 9.0m.s-1 = 10 - 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 9.0); + + v = -9.9; + limiter.limit_acceleration(v, -10., 0.5); + // check if the robot speed is now -9.0m.s-1 = -10 + 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -9.0); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit(v, 10.0, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit(v, -10.0, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } +} + +TEST(SpeedLimiterTest, testJerkLimits) +{ + { + // test max_jerk + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 2.5m.s-1 = 5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -2.5m.s-1 = -5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } + { + // test min_jerk + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + // acceleration is limiting, not jerk + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} From 2e5791759c1f02d831766fc60a7af37d65c78ecb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 13:17:13 +0100 Subject: [PATCH 22/36] test_nodes: catch keyboard interrupt and add simple launch tests (#1369) --- .../publisher_forward_position_controller.py | 13 ++- .../publisher_joint_trajectory_controller.py | 13 ++- ros2_controllers_test_nodes/setup.py | 3 +- .../rrbot_forward_position_publisher.yaml | 11 ++ .../rrbot_joint_trajectory_publisher.yaml | 24 ++++ ...sher_forward_position_controller_launch.py | 107 ++++++++++++++++++ ...sher_joint_trajectory_controller_launch.py | 106 +++++++++++++++++ 7 files changed, 269 insertions(+), 8 deletions(-) create mode 100644 ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py create mode 100644 ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index bb6add77ef..51582a90d4 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -68,9 +68,16 @@ def main(args=None): publisher_forward_position = PublisherForwardPosition() - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_forward_position) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_forward_position.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 27f28da1be..91d39855aa 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -184,9 +184,16 @@ def main(args=None): publisher_joint_trajectory = PublisherJointTrajectory() - rclpy.spin(publisher_joint_trajectory) - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_joint_trajectory) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_joint_trajectory.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index fb26e9c5f7..3900392a10 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -25,8 +25,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), - ("share/" + package_name, glob("launch/*.launch.py")), - ("share/" + package_name + "/configs", glob("configs/*.*")), + ("share/" + package_name + "/test", glob("test/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml new file mode 100644 index 0000000000..879ad34ab9 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0.0, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 0000000000..7dd8304134 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_position_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_position_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py new file mode 100644 index 0000000000..8ebb2df7b3 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -0,0 +1,107 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_forward_position_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_forward_position_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_forward_position_controller not found!" + + def test_check_if_topic_published(self): + topic = "/forward_position_controller/commands" + wait_for_topics = WaitForTopics([(topic, Float64MultiArray)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.data) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py new file mode 100644 index 0000000000..d8d6e2a577 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -0,0 +1,106 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_joint_trajectory_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_position_trajectory_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_position_trajectory_controller not found!" + + def test_check_if_topic_published(self): + topic = "/joint_trajectory_position_controller/joint_trajectory" + wait_for_topics = WaitForTopics([(topic, JointTrajectory)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.joint_names) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) From 0590c6ac2f916b13ce5842913b8811a32fd4ffab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:07:19 +0100 Subject: [PATCH 23/36] Gpio command controller (#1251) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: m.bednarczyk Co-authored-by: Maciej Bednarczyk Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich Co-authored-by: Christoph Froehlich Co-authored-by: Sai Kishor Kothakota --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + gpio_controllers/CMakeLists.txt | 115 +++ gpio_controllers/doc/userdoc.rst | 57 ++ gpio_controllers/gpio_controllers_plugin.xml | 7 + .../gpio_command_controller.hpp | 114 +++ .../gpio_controllers/visibility_control.h | 49 ++ gpio_controllers/package.xml | 30 + .../src/gpio_command_controller.cpp | 416 +++++++++++ .../gpio_command_controller_parameters.yaml | 34 + .../test/test_gpio_command_controller.cpp | 693 ++++++++++++++++++ .../test_load_gpio_command_controller.cpp | 37 + ros2_controllers/package.xml | 1 + 13 files changed, 1558 insertions(+) create mode 100644 gpio_controllers/CMakeLists.txt create mode 100644 gpio_controllers/doc/userdoc.rst create mode 100644 gpio_controllers/gpio_controllers_plugin.xml create mode 100644 gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp create mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h create mode 100644 gpio_controllers/package.xml create mode 100644 gpio_controllers/src/gpio_command_controller.cpp create mode 100644 gpio_controllers/src/gpio_command_controller_parameters.yaml create mode 100644 gpio_controllers/test/test_gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_load_gpio_command_controller.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d5b956aa8a..0a9c6ec485 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,6 +56,7 @@ The controllers are using `common hardware interface definitions`_, and may use PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Broadcasters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 688c35724d..b1db026df2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -63,3 +63,7 @@ steering_controllers_library tricycle_controller ************************ * tricycle_controller now uses generate_parameter_library (`#957 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..6ea97e04ca --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + control_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..199a4bd208 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,57 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). + +gpio_command_controller +----------------------------- +gpio_command_controller let the user expose command interfaces of given GPIO interfaces and publishes state interfaces of the configured command interfaces. + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. + + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set, else the one present in the URDF. + +.. note:: + + When no state interface is provided in the param file, the controller will try to use state_interfaces from ros2_control's config placed in the URDF for configured gpio interfaces. + However, command interfaces will not be configured based on the available URDF setup. + +.. code-block:: yaml + + gpio_command_controller: + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 + state_interfaces: + Gpio2: + - interfaces: + - ana.1 + - ana.2 + +With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..03fd3e1ee0 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..5149c74a1c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,114 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_command_controller_parameters.hpp" +#include "gpio_controllers/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +namespace gpio_controllers +{ +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; +using StateInterfaces = + std::vector>; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void store_command_interface_types(); + void store_state_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + void apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; + void apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, + std::size_t command_interface_index) const; + bool should_broadcast_all_interfaces_of_configured_gpios() const; + void set_all_state_interfaces_of_configured_gpios(); + InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; + bool update_dynamic_map_parameters(); + std::vector get_gpios_from_urdf() const; + +protected: + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + MapOfReferencesToStateInterfaces state_interfaces_map_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; + + std::shared_ptr param_listener_{}; + gpio_command_controller_parameters::Params params_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..a735a1621c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..9b98746545 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,30 @@ + + + + gpio_controllers + 4.15.0 + Controllers to interact with gpios. + Maciej Bednarczyk + Wiktor Bajor + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + control_msgs + realtime_tools + generate_parameter_library + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..a509eae774 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,416 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "gpio_controllers/gpio_command_controller.hpp" + +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} + +std::vector extract_gpios_from_hardware_info( + const std::vector & hardware_infos) +{ + std::vector result; + for (const auto & hardware_info : hardware_infos) + { + std::copy( + hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); + } + return result; +} +} // namespace + +namespace gpio_controllers +{ + +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} + +CallbackReturn GpioCommandController::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try +{ + if (!update_dynamic_map_parameters()) + { + return controller_interface::CallbackReturn::ERROR; + } + + store_command_interface_types(); + store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } + + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); + if ( + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + return true; +} + +void GpioCommandController::store_command_interface_types() +{ + for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const +{ + auto are_interfaces_empty = [](const auto & interfaces) + { return interfaces.second.interfaces.empty(); }; + return std::all_of( + params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), + are_interfaces_empty); +} + +std::vector GpioCommandController::get_gpios_from_urdf() const +try +{ + return extract_gpios_from_hardware_info( + hardware_interface::parse_control_resources_from_urdf(get_robot_description())); +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); + return {}; +} + +void GpioCommandController::set_all_state_interfaces_of_configured_gpios() +{ + const auto gpios{get_gpios_from_urdf()}; + for (const auto & gpio_name : params_.gpios) + { + for (const auto & gpio : gpios) + { + if (gpio_name == gpio.name) + { + std::transform( + gpio.state_interfaces.begin(), gpio.state_interfaces.end(), + std::back_insert_iterator(state_interface_types_), + [&gpio_name](const auto & interface_name) + { return gpio_name + '/' + interface_name.name; }); + } + } + } +} + +void GpioCommandController::store_state_interface_types() +{ + if (should_broadcast_all_interfaces_of_configured_gpios()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "State interfaces are not configured. All available interfaces of configured GPIOs will be " + "broadcasted."); + set_all_state_interfaces_of_configured_gpios(); + return; + } + + for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::initialize_gpio_state_msg() +{ + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) + { + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; + gpio_state_msg.interface_values[gpio_index].interface_names = + get_gpios_state_interfaces_names(gpio_name); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + gpio_state_msg.interface_values[gpio_index].interface_names.size(), + std::numeric_limits::quiet_NaN()); + } +} + +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) +{ + std::unordered_map> map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } + } + return map; +} + +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) +{ + if (!(configured_interfaces.size() == interfaces_from_params.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); + } + print_interface(get_node()->get_logger(), configured_interfaces); + return false; + } + return true; +} + +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + { + return controller_interface::return_type::OK; + } + + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + apply_command(gpio_commands, gpio_index, command_interface_index); + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const +{ + const auto full_command_interface_name = + gpio_commands.interface_groups[gpio_index] + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + +void GpioCommandController::update_gpios_states() +{ + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + { + return; + } + + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + apply_state_value(gpio_state_msg, gpio_index, interface_index); + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +void GpioCommandController::apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const +{ + const auto interface_name = + state_msg.interface_groups[gpio_index] + '/' + + state_msg.interface_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); + } +} + +} // namespace gpio_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..0c6aade35f --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,34 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + read_only: true, + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of command interfaces for each gpio.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..ec0e0f1228 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,693 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +namespace +{ +const auto hardware_resources_with_gpio = + R"( + + + + + +)"; + +const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_with_gpio) + + std::string(ros2_control_test_assets::urdf_tail); +} // namespace + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +class GpioCommandControllerTestSuite : public ::testing::Test +{ +public: + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + } + + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.interface_groups = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; + } + + FriendGpioCommandController controller; + + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; + + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + std::unique_ptr node; +}; + +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) +{ + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + controller.define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); + assert_default_command_and_state_values(); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto command_pub = node->create_publisher( + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) +{ + const std::vector single_gpio{"gpio1"}; + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", single_gpio}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..379a53b048 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ccde6cf812..4969658788 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -24,6 +24,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller From 4f447f4be74da751462a9f8e74e1b0b45c2adb67 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 18 Nov 2024 19:50:08 +0100 Subject: [PATCH 24/36] Add few warning flags to error in all ros2_controllers packages and fix tests (#1370) --- ackermann_steering_controller/CMakeLists.txt | 3 +- .../test_ackermann_steering_controller.hpp | 7 +-- admittance_controller/CMakeLists.txt | 3 +- .../test/test_admittance_controller.hpp | 18 +++---- bicycle_steering_controller/CMakeLists.txt | 3 +- .../test/test_bicycle_steering_controller.hpp | 18 ++++--- diff_drive_controller/CMakeLists.txt | 4 +- effort_controllers/CMakeLists.txt | 3 +- .../CMakeLists.txt | 3 +- .../test_force_torque_sensor_broadcaster.cpp | 4 +- .../test_force_torque_sensor_broadcaster.hpp | 2 +- forward_command_controller/CMakeLists.txt | 3 +- gripper_controllers/CMakeLists.txt | 4 +- imu_sensor_broadcaster/CMakeLists.txt | 3 +- .../test/test_imu_sensor_broadcaster.hpp | 2 +- joint_state_broadcaster/CMakeLists.txt | 3 +- joint_trajectory_controller/CMakeLists.txt | 4 +- parallel_gripper_controller/CMakeLists.txt | 3 +- pid_controller/CMakeLists.txt | 4 +- pose_broadcaster/CMakeLists.txt | 3 +- .../test/test_pose_broadcaster.hpp | 2 +- position_controllers/CMakeLists.txt | 3 +- range_sensor_broadcaster/CMakeLists.txt | 4 +- .../src/range_sensor_broadcaster.cpp | 8 +-- .../test/test_range_sensor_broadcaster.cpp | 50 +++++++++---------- .../test/test_range_sensor_broadcaster.hpp | 1 + steering_controllers_library/CMakeLists.txt | 3 +- .../test_steering_controllers_library.hpp | 7 +-- tricycle_controller/CMakeLists.txt | 3 +- tricycle_steering_controller/CMakeLists.txt | 3 +- .../test_tricycle_steering_controller.hpp | 18 +++---- velocity_controllers/CMakeLists.txt | 3 +- 32 files changed, 116 insertions(+), 86 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 83edf7c157..2ffc413633 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 59637a072f..c45601d701 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -293,9 +293,10 @@ class AckermannSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - 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/velocity"}; + 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/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index cd16714a31..477a343776 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 4f4635d861..7ee56b8c11 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -371,15 +371,15 @@ class AdmittanceControllerTest : public ::testing::Test const std::string fixed_world_frame_ = "fixed_world_frame"; const std::string sensor_frame_ = "link_6"; - std::array admittance_selected_axes_ = {true, true, true, true, true, true}; - std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427, - 2.828427, 2.828427, 2.828427}; - std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; - - std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array admittance_selected_axes_ = {{true, true, true, true, true, true}}; + std::array admittance_mass_ = {{5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; + std::array admittance_damping_ratio_ = { + {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}}; + std::array admittance_stiffness_ = {{214.1, 214.2, 214.3, 214.4, 214.5, 214.6}}; + + std::array joint_command_values_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + std::array joint_state_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; + std::array fts_state_values_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; std::vector fts_state_names_; std::vector state_itfs_; diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index db1f5cf4ba..1f3a4599cc 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 65f1691a3b..0253078bb9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -247,12 +247,13 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + std::vector rear_wheels_names_ = {{"rear_wheel_joint"}}; + std::vector front_wheels_names_ = {{"steering_axis_joint"}}; + std::vector joint_names_ = {{rear_wheels_names_[0], front_wheels_names_[0]}}; - std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector rear_wheels_preceeding_names_ = {{"pid_controller/rear_wheel_joint"}}; + std::vector front_wheels_preceeding_names_ = { + {"pid_controller/steering_axis_joint"}}; std::vector preceeding_joint_names_ = { rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; @@ -260,9 +261,10 @@ class BicycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - 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/velocity"}; + 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/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 4eeb98b9d2..4b3ff4f77f 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index eae8642fb6..e79015fbbf 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 02c343f050..d9b005e650 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index e436beb2e5..26f959e88f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -282,8 +282,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets { SetUpFTSBroadcaster(); - std::array force_offsets = {10.0, 30.0, -50.0}; - std::array torque_offsets = {1.0, -1.2, -5.2}; + std::array force_offsets = {{10.0, 30.0, -50.0}}; + std::array torque_offsets = {{1.0, -1.2, -5.2}}; // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe7fb0c174..5d485d9d12 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -55,7 +55,7 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "fts_sensor"; const std::string frame_id_ = "fts_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 4885c69c8a..bf027866d6 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index c4a85dd453..4e9c72f79b 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,9 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 5539dea9ff..38f0adbb54 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 0f3286c302..70e01d593c 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -54,7 +54,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; hardware_interface::StateInterface imu_orientation_x_{ sensor_name_, "orientation.x", &sensor_values_[0]}; hardware_interface::StateInterface imu_orientation_y_{ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cc8dc18bf6..a75bbfa8f9 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 438661f7bf..ec142c72f3 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index cb0f4fafc9..3d9be8dfac 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(parallel_gripper_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index f9a9abce89..ed9bdcd8cf 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() if(WIN32) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 46028cf258..3cc434d2d0 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -6,7 +6,8 @@ project(pose_broadcaster if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index a164b2c6ac..10b9c03d1c 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -39,7 +39,7 @@ class PoseBroadcasterTest : public ::testing::Test const std::string frame_id_ = "pose_base_frame"; const std::string tf_child_frame_id_ = "pose_frame"; - std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + std::array pose_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}}; hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 18f3cb313a..e76b76555e 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 6f21607c9c..d70614ea53 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,9 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index 7c6d714be3..4ff817b2d3 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -75,10 +75,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->msg_.radiation_type = params_.radiation_type; - realtime_publisher_->msg_.field_of_view = params_.field_of_view; - realtime_publisher_->msg_.min_range = params_.min_range; - realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->msg_.radiation_type = static_cast(params_.radiation_type); + realtime_publisher_->msg_.field_of_view = static_cast(params_.field_of_view); + realtime_publisher_->msg_.min_range = static_cast(params_.min_range); + realtime_publisher_->msg_.max_range = static_cast(params_.max_range); // \note The versions conditioning is added here to support the source-compatibility with Humble #if SENSOR_MSGS_VERSION_MAJOR >= 5 realtime_publisher_->msg_.variance = params_.variance; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 59d27ebc0c..052c0384d3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -208,11 +208,11 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); #endif @@ -227,30 +227,30 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) sensor_msgs::msg::Range range_msg; - sensor_range_ = 0.10; + sensor_range_ = 0.10f; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 4.0; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } @@ -268,13 +268,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 6.0; @@ -282,13 +282,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp index 10696d071f..b7ffa7fe4a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -38,6 +38,7 @@ class RangeSensorBroadcasterTest : public ::testing::Test // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml const std::string sensor_name_ = "range_sensor"; const std::string frame_id_ = "range_sensor_frame"; + const std::string interface_name_ = "range"; const double field_of_view_ = 0.1; const int radiation_type_ = 1; diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index e2bfdbab71..fc79d54b7c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -3,7 +3,8 @@ project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 93ee823e0f..ff0ab972d5 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -314,10 +314,11 @@ class SteeringControllersLibraryFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - 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_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/velocity"}; + 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_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 7afe150c43..ee877272ab 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 24b4cd1a22..9c82ee3574 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 3b7a053937..c5dd71099f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -260,15 +260,15 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = { + std::vector rear_wheels_names_{"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_{"steering_axis_joint"}; + std::vector joint_names_{ rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector rear_wheels_preceeding_names_{ "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; - std::vector preceeding_joint_names_ = { + std::vector front_wheels_preceeding_names_{"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_{ rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], front_wheels_preceeding_names_[0]}; @@ -278,9 +278,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - 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/velocity"}; + 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/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index a39cd162fd..feb4eae74f 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 3b600f28067f2a2146861db58f3d7c10925e492b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 20:22:09 +0100 Subject: [PATCH 25/36] Add compatibility build for humble+jazzy distro (#1368) --- .../workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/jazzy-semi-binary-build.yml | 2 +- ...ling-compatibility-humble-binary-build.yml | 48 +++++++++++++++++++ ...lling-compatibility-jazzy-binary-build.yml | 48 +++++++++++++++++++ .../workflows/rolling-semi-binary-build.yml | 2 +- 6 files changed, 100 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/rolling-compatibility-humble-binary-build.yml create mode 100644 .github/workflows/rolling-compatibility-jazzy-binary-build.yml diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 4246d18160..c53c46032a 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: humble ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 3aca5e5b70..a0782e50bb 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -40,7 +40,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: iron ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index ffa4f914b9..a462d9040a 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: jazzy ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..7aab02fa62 --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..37ffbb84a4 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 872c509931..db0dd1fe64 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: rolling ros_repo: ${{ matrix.ROS_REPO }} From 29e84b977be4b8fda791e17fd1ba47ec5b2ef3ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 22:11:03 +0100 Subject: [PATCH 26/36] Remove iron workflows and update readme (#1338) Co-authored-by: Dr. Denis --- .github/dependabot.yml | 7 --- .github/mergify.yml | 9 ---- .github/workflows/iron-abi-compatibility.yml | 27 ---------- .github/workflows/iron-binary-build.yml | 50 ------------------- .github/workflows/iron-check-docs.yml | 18 ------- .github/workflows/iron-coverage-build.yml | 38 -------------- .github/workflows/iron-debian-build.yml | 30 ----------- .github/workflows/iron-pre-commit.yml | 13 ----- .../workflows/iron-rhel-semi-binary-build.yml | 29 ----------- .github/workflows/iron-semi-binary-build.yml | 48 ------------------ .github/workflows/iron-source-build.yml | 28 ----------- README.md | 1 - ros2_controllers-not-released.iron.repos | 6 --- ros2_controllers.iron.repos | 21 -------- 14 files changed, 325 deletions(-) delete mode 100644 .github/workflows/iron-abi-compatibility.yml delete mode 100644 .github/workflows/iron-binary-build.yml delete mode 100644 .github/workflows/iron-check-docs.yml delete mode 100644 .github/workflows/iron-coverage-build.yml delete mode 100644 .github/workflows/iron-debian-build.yml delete mode 100644 .github/workflows/iron-pre-commit.yml delete mode 100644 .github/workflows/iron-rhel-semi-binary-build.yml delete mode 100644 .github/workflows/iron-semi-binary-build.yml delete mode 100644 .github/workflows/iron-source-build.yml delete mode 100644 ros2_controllers-not-released.iron.repos delete mode 100644 ros2_controllers.iron.repos diff --git a/.github/dependabot.yml b/.github/dependabot.yml index aafd67c236..f5e9921f23 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,10 +18,3 @@ updates: schedule: interval: "weekly" target-branch: "humble" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "weekly" - target-branch: "iron" diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..c2eda7385b 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,15 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion - conditions: - - base=master - - "label=backport-iron" - actions: - backport: - branches: - - iron - - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml deleted file mode 100644 index be1aec6680..0000000000 --- a/.github/workflows/iron-abi-compatibility.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: Iron - ABI Compatibility Check -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '.github/workflows/iron-abi-compatibility.yml' - - '**.yaml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: iron - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml deleted file mode 100644 index 0d2f0ff7f3..0000000000 --- a/.github/workflows/iron-binary-build.yml +++ /dev/null @@ -1,50 +0,0 @@ -name: Iron Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [main, testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers-not-released.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml deleted file mode 100644 index f842902055..0000000000 --- a/.github/workflows/iron-check-docs.yml +++ /dev/null @@ -1,18 +0,0 @@ -name: Iron Check Docs - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.rst' - - '**.md' - - '**.yaml' - -jobs: - check-docs: - name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron - with: - ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml deleted file mode 100644 index e1d2e84bc8..0000000000 --- a/.github/workflows/iron-coverage-build.yml +++ /dev/null @@ -1,38 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - -jobs: - coverage_iron: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master - secrets: inherit - with: - ros_distro: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml deleted file mode 100644 index ebd2f46c18..0000000000 --- a/.github/workflows/iron-debian-build.yml +++ /dev/null @@ -1,30 +0,0 @@ -name: Debian Iron Source Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-debian-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_debian: - name: Iron debian build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml deleted file mode 100644 index a128958031..0000000000 --- a/.github/workflows/iron-pre-commit.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Pre-Commit - Iron - -on: - workflow_dispatch: - pull_request: - branches: - - iron - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: iron diff --git a/.github/workflows/iron-rhel-semi-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml deleted file mode 100644 index 109dac7550..0000000000 --- a/.github/workflows/iron-rhel-semi-binary-build.yml +++ /dev/null @@ -1,29 +0,0 @@ -name: RHEL Iron Binary Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-rhel-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_rhel_binary: - name: Iron RHEL binary build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml deleted file mode 100644 index a0782e50bb..0000000000 --- a/.github/workflows/iron-semi-binary-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Iron Semi-Binary Build -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml deleted file mode 100644 index c54ead7877..0000000000 --- a/.github/workflows/iron-source-build.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: Iron Source Build -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-source-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' - -jobs: - source: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master - with: - ros_distro: iron - ref: iron - ros2_repo_branch: iron - os_name: ubuntu-22.04 diff --git a/README.md b/README.md index 57a9ecdda1..28a501dcdc 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types diff --git a/ros2_controllers-not-released.iron.repos b/ros2_controllers-not-released.iron.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.iron.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.iron.repos b/ros2_controllers.iron.repos deleted file mode 100644 index 7f2db052cb..0000000000 --- a/ros2_controllers.iron.repos +++ /dev/null @@ -1,21 +0,0 @@ -repositories: - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: iron - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master From 79358e38100139cd1d91ba8d090a388677d464b3 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 19 Nov 2024 09:04:51 +0100 Subject: [PATCH 27/36] Add Mecanum Drive Controller (#512) --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + mecanum_drive_controller/CMakeLists.txt | 110 ++++ mecanum_drive_controller/doc/userdoc.rst | 61 ++ .../mecanum_drive_controller.hpp | 162 +++++ .../mecanum_drive_controller/odometry.hpp | 104 ++++ .../visibility_control.h | 49 ++ .../mecanum_drive_controller.xml | 7 + mecanum_drive_controller/package.xml | 50 ++ .../src/mecanum_drive_controller.cpp | 527 ++++++++++++++++ .../src/mecanum_drive_controller.yaml | 148 +++++ mecanum_drive_controller/src/odometry.cpp | 124 ++++ .../test/mecanum_drive_controller_params.yaml | 19 + ...um_drive_controller_preceeding_params.yaml | 24 + .../test_load_mecanum_drive_controller.cpp | 52 ++ .../test/test_mecanum_drive_controller.cpp | 576 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 317 ++++++++++ ...st_mecanum_drive_controller_preceeding.cpp | 99 +++ ros2_controllers/package.xml | 1 + steering_controllers_library/doc/userdoc.rst | 2 +- 20 files changed, 2436 insertions(+), 1 deletion(-) create mode 100644 mecanum_drive_controller/CMakeLists.txt create mode 100644 mecanum_drive_controller/doc/userdoc.rst create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h create mode 100644 mecanum_drive_controller/mecanum_drive_controller.xml create mode 100644 mecanum_drive_controller/package.xml create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.yaml create mode 100644 mecanum_drive_controller/src/odometry.cpp create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_params.yaml create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml create mode 100644 mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 0a9c6ec485..fe09839ce5 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -28,6 +28,7 @@ Controllers for Wheeled Mobile Robots :titlesonly: Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b1db026df2..974bca880f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -50,6 +50,10 @@ joint_trajectory_controller * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). +mecanum_drive_controller +************************ +* 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..48bac58fe7 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.16) +project(mecanum_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(mecanum_drive_controller_parameters + src/mecanum_drive_controller.yaml +) + +add_library( + mecanum_drive_controller + SHARED + src/mecanum_drive_controller.cpp + src/odometry.cpp +) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) +target_include_directories(mecanum_drive_controller PUBLIC + "$" + "$") +target_link_libraries(mecanum_drive_controller PUBLIC + mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mecanum_drive_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) + ament_target_dependencies(test_load_mecanum_drive_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + target_include_directories(test_mecanum_drive_controller PRIVATE include) + target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/mecanum_drive_controller +) + +install( + TARGETS mecanum_drive_controller mecanum_drive_controller_parameters + EXPORT export_mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..37c8d7d0e7 --- /dev/null +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -0,0 +1,61 @@ +.. _mecanum_drive_controller_userdoc: + +mecanum_drive_controller +========================= + +Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. +In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. + +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +Commands +,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +States +,,,,,,, +- / [double] # in [rad] or [rad/s] + ..note :: + + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. + + +Subscribers +,,,,,,,,,,,, +Used when the controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/MecanumDriveControllerState] + +Parameters +,,,,,,,,,,, + +For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization, see the ``test`` folder of the controller's package. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp new file mode 100644 index 0000000000..0fd7f6af29 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ +#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "mecanum_drive_controller/odometry.hpp" +#include "mecanum_drive_controller/visibility_control.h" +#include "mecanum_drive_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +namespace mecanum_drive_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 4; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 4; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 3; + +class MecanumDriveController : public controller_interface::ChainableControllerInterface +{ +public: + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + MecanumDriveController(); + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using OdomStateMsg = nav_msgs::msg::Odometry; + using TfStateMsg = tf2_msgs::msg::TFMessage; + using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; + +protected: + std::shared_ptr param_listener_; + mecanum_drive_controller::Params params_; + + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + enum WheelIndex : std::size_t + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_RIGHT = 2, + REAR_LEFT = 3 + }; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + */ + std::vector command_joint_names_; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + * If parameters for state joint names are *not* defined, this list is the same as + * `command_joint_names_`. + */ + std::vector state_joint_names_; + + // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. + // used for preceding controller + std::vector reference_names_; + + // Command subscribers and Controller State, odom state, tf state publishers + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + using OdomStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + std::unique_ptr rt_odom_state_publisher_; + + using TfStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + Odometry odometry_; + +private: + // callback for topic interface + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); + + double velocity_in_center_frame_linear_x_; // [m/s] + double velocity_in_center_frame_linear_y_; // [m/s] + double velocity_in_center_frame_angular_z_; // [rad/s] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp new file mode 100644 index 0000000000..ac1ad060dd --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include "geometry_msgs/msg/twist.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef std::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + Odometry(); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const rclcpp::Time & time, std::array base_frame_offset); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel_front_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_right_vel Wheel velocity [rad/s] + /// \param wheel_front_right_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); + + /// \return position (x component) [m] + double getX() const { return position_x_in_base_frame_; } + /// \return position (y component) [m] + double getY() const { return position_y_in_base_frame_; } + /// \return orientation (z component) [m] + double getRz() const { return orientation_z_in_base_frame_; } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const { return velocity_in_base_frame_linear_x; } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const { return velocity_in_base_frame_linear_y; } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return velocity_in_base_frame_angular_z; + ; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param + /// (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); + +private: + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Reference frame (wrt to center frame). [x, y, theta] + std::array base_frame_offset_; + + /// Current pose: + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] + double orientation_z_in_base_frame_; // [rad] + + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] + + /// Wheels kinematic parameters [m]: + /// lx and ly represent the distance from the robot's center to the wheels + /// projected on the x and y axis with origin at robots center respectively, + /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + double sum_of_robot_center_projection_on_X_Y_axis_; + double wheels_radius_; // [m] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h new file mode 100644 index 0000000000..3222b2fa52 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml new file mode 100644 index 0000000000..c012ad53ca --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -0,0 +1,7 @@ + + + + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 0000000000..97aadf4327 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,50 @@ + + + + mecanum_drive_controller + 0.0.0 + Implementation of mecanum drive controller for 4 wheel drive. + + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + Giridhar Bukka + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 0000000000..2fd5fdbdcb --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,527 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "mecanum_drive_controller/mecanum_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mecanum_drive_controller +{ +MecanumDriveController::MecanumDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MecanumDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::size_t index, const std::string & command_joint_name, + const std::string & state_joint_name) + { + command_joints[index] = command_joint_name; + if (state_joint_name.empty()) + { + state_joints[index] = command_joint_name; + } + else + { + state_joints[index] = state_joint_name; + } + }; + + command_joint_names_.resize(4); + state_joint_names_.resize(4); + + // The joint names are sorted according to the order documented in the header file! + prepare_lists_with_joint_names( + FRONT_LEFT, params_.front_left_wheel_command_joint_name, + params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_RIGHT, params_.front_right_wheel_command_joint_name, + params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_RIGHT, params_.rear_right_wheel_command_joint_name, + params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_LEFT, params_.rear_left_wheel_command_joint_name, + params_.rear_left_wheel_state_joint_name); + + // Set wheel params for the odometry computation + odometry_.setWheelsParams( + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, + params_.kinematics.wheels_radius); + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = + get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = + get_node()->create_publisher("~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // controller State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void MecanumDriveController::reference_callback(const std::shared_ptr msg) +{ + // If no timestamp provided, use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + // Check the timeout condition + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older by %.10f than allowed timeout (%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + + reset_controller_reference_msg(msg, get_node()); + } +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration MecanumDriveController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joint_names_.size()); + + for (const auto & joint : state_joint_names_) + { + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return state_interfaces_config; +} + +std::vector +MecanumDriveController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn MecanumDriveController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + bool value_set_no_error = true; + for (size_t i = 0; i < NR_CMD_ITFS; ++i) + { + value_set_no_error &= + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + if (!value_set_no_error) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + return controller_interface::CallbackReturn::FAILURE; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.linear.y; + reference_interfaces_[2] = current_ref->twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + } + else + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MecanumDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // FORWARD KINEMATICS (odometry). + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + + if ( + !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && + !std::isnan(wheel_rear_right_state_vel) && !std::isnan(wheel_front_right_state_vel)) + { + // Estimate twist (using joint information) and integrate + odometry_.update( + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, + wheel_front_right_state_vel, period.seconds()); + } + + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic `~/reference`) is a body twist. + if ( + !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && + !std::isnan(reference_interfaces_[2])) + { + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); + /// \note The variables meaning: + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from + /// base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to + /// transform from base frame to center frame + + tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); + tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = + rotation_from_base_to_center * + tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( + params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); + + velocity_in_center_frame_linear_x_ = + velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y_ = + velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + + const double wheel_front_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_front_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + + // Set wheels velocities - The joint names are sorted according to the order documented in the + // header file! + const bool value_set_error = + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel) && + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel) && + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel) && + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + else + { + const bool value_set_error = command_interfaces_[FRONT_LEFT].set_value(0.0) && + command_interfaces_[FRONT_RIGHT].set_value(0.0) && + command_interfaces_[REAR_RIGHT].set_value(0.0) && + command_interfaces_[REAR_LEFT].set_value(0.0); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getRz()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); + rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.front_left_wheel_velocity = + state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = + state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = + state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = + state_interfaces_[REAR_LEFT].get_value(); + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace mecanum_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml new file mode 100644 index 0000000000..896b731953 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -0,0 +1,148 @@ +mecanum_drive_controller: + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + kinematics: + base_frame_offset: + x: { + type: double, + default_value: 0.0, + description: "Base frame offset along X axis of base_frame (base_link frame)." , + read_only: true, + } + y: { + type: double, + default_value: 0.0, + description: "Base frame offset along Y axis of base_frame (base_link frame)." , + read_only: true, + } + theta: { + type: double, + default_value: 0.0, + description: "Base frame offset along Theta axis of base_frame (base_link frame).", + read_only: true, + } + + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + sum_of_robot_center_projection_on_X_Y_axis: { + type: double, + default_value: 0.0, + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled?", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of pose covariance matrix.", + read_only: false, + } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..bb873fcfdb --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "mecanum_drive_controller/odometry.hpp" + +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace mecanum_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + position_x_in_base_frame_(0.0), + position_y_in_base_frame_(0.0), + orientation_z_in_base_frame_(0.0), + velocity_in_base_frame_linear_x(0.0), + velocity_in_base_frame_linear_y(0.0), + velocity_in_base_frame_angular_z(0.0), + sum_of_robot_center_projection_on_X_Y_axis_(0.0), + wheels_radius_(0.0) +{ +} + +void Odometry::init( + const rclcpp::Time & time, std::array base_frame_offset) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt) +{ + /// We cannot estimate the speed with very small time intervals: + // const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) return false; // Interval too small to integrate with + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity + /// at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and + /// let the user perform post-processing at will. + /// We prefer this way of doing as filtering introduces delay (which makes it difficult + /// to interpret and compare behavior curves). + + /// \note The variables meaning: + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform + /// from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, + /// to transform from center frame to base frame + + double velocity_in_center_frame_linear_x = + 0.25 * wheels_radius_ * + (wheel_front_left_vel + wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_linear_y = + 0.25 * wheels_radius_ * + (-wheel_front_left_vel + wheel_rear_left_vel - wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_angular_z = + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * + (-wheel_front_left_vel - wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + + tf2::Quaternion orientation_R_c_b; + orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); + + tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ = + angular_transformation_from_center_2_base * + tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base = + angular_transformation_from_center_2_base * + tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + velocity_in_base_frame_linear_x = + velocity_in_center_frame_w_r_t_base_frame_.x() + + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = + velocity_in_center_frame_w_r_t_base_frame_.y() - + linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame , unlike the twist which is + /// expressed in the body frame. + orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; + + tf2::Quaternion orientation_R_b_odom; + orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_); + + tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = + angular_transformation_from_base_2_odom * + tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); + + position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; + + return true; +} + +void Odometry::setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius) +{ + sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml new file mode 100644 index 0000000000..580b4058d7 --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -0,0 +1,19 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..c4e5bb391a --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -0,0 +1,24 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.1 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..16fa3e815a --- /dev/null +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMecanumDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/mecanum_drive_controller_params.yaml"; + + cm.set_parameter({"test_mecanum_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_mecanum_drive_controller.type", "mecanum_drive_controller/MecanumDriveController"}); + + ASSERT_NE(cm.load_controller("test_mecanum_drive_controller"), nullptr); + + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..5327e890e9 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -0,0 +1,576 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +namespace +{ +// Floating-point value comparison threshold +const double EPS = 1e-3; +} // namespace + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 0.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.9); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check command itfs configuration + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check state itfs configuration + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs configuration, + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + } +} + +TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// when update is called expect the previously set reference before calling update, +// inside the controller state message +TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + controller_->reference_interfaces_[0] = 1.5; + + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); +} + +// reference_interfaces and command_interfaces values depend on the reference_msg, +// the below test shows two cases when reference_msg is not received and when it is received. +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + // no reference_msg sent + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); + ASSERT_EQ(msg.back_left_wheel_velocity, 0.1); + ASSERT_EQ(msg.back_right_wheel_velocity, 0.1); +} + +// when too old msg is sent expect reference msg reset +TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = *(controller_->input_ref_.readFromNonRT()); + auto old_timestamp = reference->header.stamp; + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); +} + +// when time stamp is zero expect that time stamp is set to current time stamp +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(rclcpp::Time(0)); + + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); +} + +// when the reference_msg has valid timestamp then the timeout check in reference_callback() +// shall pass and reference_msg is not reset +TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +// when not in chainable mode and ref_msg_timedout expect +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect +// command_interfaces are set to valid command values +TEST_F( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // BACK Left vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * + // 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + +// when in chained mode the reference_interfaces of chained controller and command_interfaces +// of preceding controller point to same memory location, hence reference_interfaces are not +// exclusively set by the update method of chained controller +TEST_F( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + // imitating preceding controllers command_interfaces setting reference_interfaces of chained + // controller. + controller_->reference_interfaces_[0] = 3.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 6.0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + } +} + +// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces +// are calculated to valid values and reference_interfaces are unset +TEST_F( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); +} + +TEST_F( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + // reference_callback() is called implicitly when publish_commands() is called. + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp new file mode 100644 index 0000000000..5f7cf36dc7 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -0,0 +1,317 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEST_MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; +using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController +{ + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); + FRIEND_TEST( + MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return mecanum_drive_controller::MecanumDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + size_t get_front_left_wheel_index() { return WheelIndex::FRONT_LEFT; } + + size_t get_front_right_wheel_index() { return WheelIndex::FRONT_RIGHT; } + + size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; } + + size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MecanumDriveControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/reference", rclcpp::SystemDefaultsQoS()); + + odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); + odom_s_publisher_ = odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); + + tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); + tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + { + const auto urdf = ""; + const auto ns = ""; + ASSERT_EQ( + controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + command_joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_mecanum_drive_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + // call update to publish the test value + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, + const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; + + command_publisher_->publish(msg); + } + +protected: + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; + std::vector command_joint_names_ = { + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; + std::vector state_joint_names_ = { + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; + + // Controller-related parameters + + std::array joint_state_values_ = {{0.1, 0.1, 0.1, 0.1}}; + std::array joint_command_values_ = {{101.101, 101.101, 101.101, 101.101}}; + + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + + std::vector state_itfs_; + std::vector command_itfs_; + + double ref_timeout_ = 0.1; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Node::SharedPtr tf_odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; +}; + +#endif // TEST_MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..edbee8a702 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ( + controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); +} + +// checking if all interfaces, command and state interfaces are exported as expected +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4969658788..99ca06c77b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -28,6 +28,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller pid_controller position_controllers range_sensor_broadcaster diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 44b180162e..8889824d62 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -10,7 +10,7 @@ steering_controllers_library .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg -Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. +Library with shared functionalities for mobile robot controllers with steering drives (2 degrees of freedom), with so-called non-holonomic constraints. The library implements generic odometry and update methods and defines the main interfaces. From a5bb11bf1402c9d2e50e21e4ce7380f2b4f14218 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 11:15:31 +0100 Subject: [PATCH 28/36] Add missing deps for test_nodes (#1378) --- ros2_controllers_test_nodes/package.xml | 2 ++ .../test_publisher_forward_position_controller_launch.py | 6 ++---- .../test_publisher_joint_trajectory_controller_launch.py | 5 ++--- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c53f1b9971..0b97d6bb0c 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -21,6 +21,8 @@ trajectory_msgs python3-pytest + launch_testing_ros + launch_ros ament_python diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py index 8ebb2df7b3..3f4c9da21f 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -34,15 +34,13 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node - from std_msgs.msg import Float64MultiArray diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index d8d6e2a577..62ad25550d 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -34,13 +34,12 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory From 1bd6870a1115a0551506bbd45afd88c6ce16c2a8 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 22 Nov 2024 11:22:20 +0100 Subject: [PATCH 29/36] [JTC] Sample at t + dT instead of the beginning of the control cycle (#1306) --- .../joint_trajectory_controller.hpp | 2 + .../trajectory.hpp | 11 +- .../src/joint_trajectory_controller.cpp | 29 ++- .../src/trajectory.cpp | 8 +- .../test/test_trajectory_controller.cpp | 166 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 4 +- 6 files changed, 147 insertions(+), 73 deletions(-) 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 2f0a1f4df0..67954b8378 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 @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_next_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -124,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; + rclcpp::Duration update_period_{0, 0}; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 14373b006e..d650c369d0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,8 +69,10 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * - * This function assumes that sampling is only done at monotonically increasing \p sample_time - * for any trajectory. + * This function by default assumes that sampling is only done at monotonically increasing \p + * sample_time for any trajectory. That means, it will only search for a point matching the sample + * time after the point it has been called before. If this isn't desired, set \p + * search_monotonically_increasing to false. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: @@ -94,13 +96,16 @@ class Trajectory * description above. * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See * description above. + * \param[in] search_monotonically_increasing If set to true, the next sample call will start + * searching in the trajectory at the index of this call's result. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing = true); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bea37e02be..0fc39ecaaf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( if (has_active_trajectory()) { bool first_sample = false; + TrajectoryPointConstIter start_segment_itr, end_segment_itr; // if sampling the first time, set the point before you sample if (!traj_external_point_ptr_->is_sampled_already()) { @@ -196,11 +198,15 @@ controller_interface::return_type JointTrajectoryController::update( } } - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + // Sample expected state from the trajectory + traj_external_point_ptr_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + // Sample setpoint for next control cycle + const bool valid_point = traj_external_point_ptr_->sample( + time + update_period_, interpolation_method_, command_next_, start_segment_itr, + end_segment_itr, false); + if (valid_point) { const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); @@ -276,7 +282,7 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); @@ -286,7 +292,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[0], command_next_.positions); } if (has_velocity_command_interface_) { @@ -296,12 +302,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); } if (has_effort_command_interface_) { @@ -309,7 +315,7 @@ controller_interface::return_type JointTrajectoryController::update( } // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; + last_commanded_state_ = command_next_; } if (active_goal) @@ -867,6 +873,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (get_update_rate() == 0) + { + throw std::runtime_error("Controller's update rate is set to 0. This should not happen!"); + } + update_period_ = + rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 512b16f3c5..1c50d26c62 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -91,7 +91,8 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing) { THROW_ON_NULLPTR(trajectory_msg_) @@ -176,7 +177,10 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); - last_sample_idx_ = i; + if (search_monotonically_increasing) + { + last_sample_idx_ = i; + } return true; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f188c0f04b..21d0277541 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -53,7 +53,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -84,7 +84,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, joint_names_); @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, command_joint_names_); @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_if_conf = traj_controller_->command_interface_configuration(); @@ -162,7 +162,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) SetUpTrajectoryController(executor); // configure controller - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // cleanup controller @@ -178,6 +178,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); // This call is replacing the way parameters are set via launch auto state = traj_controller_->configure(); @@ -206,8 +207,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait for reaching the first point // controller would process the second point when deactivated below + // Since the trajectory will be sampled at time + update_period, we need to pass the time 0.15 + // seconds to sample at t=0.25 sec traj_controller_->update( - rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + rclcpp::Time(static_cast(0.15 * 1e9)), rclcpp::Duration::from_seconds(0.15)); EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { @@ -644,7 +647,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -653,7 +656,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -678,14 +697,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -745,7 +756,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -756,7 +767,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -782,14 +809,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_NEAR( state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -1548,16 +1567,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + // Check that we reached end of points_old[0] trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + if (traj_controller_->has_position_command_interface()) + { + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time rclcpp::Time new_traj_start = rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; + expected_desired = expected_actual; // robot should be standing still by now publish(time_from_start, points_new, new_traj_start); waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); @@ -1577,10 +1607,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + + if (traj_controller_->has_position_command_interface()) + { + // Check that we reached end of points_old[0] trajectory + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } // send points_new before the old trajectory is finished RCLCPP_INFO( @@ -1653,6 +1693,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1665,8 +1707,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish( time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); @@ -1690,7 +1732,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1699,10 +1741,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro 0.1); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -1721,17 +1763,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + 0.1); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -1747,13 +1791,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e rclcpp::executors::SingleThreadedExecutor executor; // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter update_rate_param("update_rate", 100); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1764,8 +1812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); @@ -1788,17 +1836,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // There should not be backward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -1817,17 +1866,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from // command (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); // There should not be a jump toward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -2098,7 +2148,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // command interfaces: empty command_interface_types_ = {}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); // command interfaces: bad_name @@ -2143,7 +2193,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"velocity"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); @@ -2152,7 +2202,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a850891331..7dff81f08f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -278,7 +278,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); } void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) @@ -324,7 +324,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(k_p, ff); - traj_controller_->get_node()->configure(); + traj_controller_->configure(); ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, From 9b344c7b5c06879b85cd4bcc135e35b5f7eedc2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 14:22:49 +0100 Subject: [PATCH 30/36] Add another dependency (#1382) --- ros2_controllers_test_nodes/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0b97d6bb0c..21eb31f6df 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -17,6 +17,7 @@ https://github.com/ros-controls/ros2_controllers/ rclpy + sensor_msgs std_msgs trajectory_msgs From d8b30f864ea70126e989cc451ddab482be8850f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Nov 2024 22:47:43 +0100 Subject: [PATCH 31/36] Rename Twist->TwistStamped (#1393) --- .../diff_drive_controller.hpp | 14 +++++------ .../src/diff_drive_controller.cpp | 23 ++++++++++--------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index ac34b81eca..74526b199c 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -45,7 +45,7 @@ namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; public: DIFF_DRIVE_CONTROLLER_PUBLIC @@ -128,20 +128,20 @@ class DiffDriveController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> + realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 66da6d6738..d21cac602d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,7 +111,7 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; + std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); if (last_command_msg == nullptr) @@ -130,7 +130,7 @@ controller_interface::return_type DiffDriveController::update( // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; + TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -318,23 +318,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( if (publish_limited_velocity_) { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = - std::make_shared>(limited_velocity_publisher_); + std::make_shared>( + limited_velocity_publisher_); } - const Twist empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); // initialize command subscriber - velocity_command_subscriber_ = get_node()->create_subscription( + velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -475,7 +476,7 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } @@ -493,7 +494,7 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; + std::queue empty; std::swap(previous_commands_, empty); registered_left_wheel_handles_.clear(); From f64c964b2ed772d7d75e8d7e5d46a1f35c8159bb Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 29 Nov 2024 16:21:01 +0100 Subject: [PATCH 32/36] JTC: sum periods (#1395) --- .../joint_trajectory_controller.hpp | 2 ++ .../src/joint_trajectory_controller.cpp | 11 ++++++++--- .../test/test_trajectory_actions.cpp | 9 ++++++--- 3 files changed, 16 insertions(+), 6 deletions(-) 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 67954b8378..2377b2c0b6 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 @@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; rclcpp::Duration update_period_{0, 0}; + rclcpp::Time traj_time_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0fc39ecaaf..82d9c94aab 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + traj_time_ = time; + } + else + { + traj_time_ += period; } // Sample expected state from the trajectory traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); // Sample setpoint for next control cycle const bool valid_point = traj_external_point_ptr_->sample( - time + update_period_, interpolation_method_, command_next_, start_segment_itr, + traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); if (valid_point) @@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update( // time_difference is // - negative until first point is reached // - counting from zero to time_from_start of next point - double time_difference = time.seconds() - segment_time_from_start.seconds(); + double time_difference = traj_time_.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 164f7d0e11..d4979deef8 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -83,12 +83,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest { // controller hardware cycle update loop auto clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = clock.now(); + auto now_time = clock.now(); + auto last_time = now_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; + auto end_time = last_time + wait; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + now_time = clock.now(); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; } }); From ff3c8a0242bb65e00f7bd65b85add08275c2bcfa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:09:50 +0100 Subject: [PATCH 33/36] Bump version of pre-commit hooks (#1401) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 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.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 36068e180aa510ab79a9c19a91f9de8ad5e74f06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 1 Dec 2024 23:02:56 +0100 Subject: [PATCH 34/36] Add explicit cast to period.count() (#1404) --- .../include/gripper_controllers/hardware_interface_adapter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 2cc24794b8..98fd5a56b1 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -165,7 +165,8 @@ class HardwareInterfaceAdapter // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = pid_->computeCommand(error_position, error_velocity, period.count()); + double command = + pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); From f9edc41a451877e292b206069aa2e8bfd25148c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 11:16:28 +0100 Subject: [PATCH 35/36] Fix RealtimeBox API changes (#1385) --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 29 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 3 +- .../tricycle_controller.hpp | 1 + .../src/tricycle_controller.cpp | 26 +++++++++-------- .../test/test_tricycle_controller.cpp | 3 +- 6 files changed, 35 insertions(+), 28 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 74526b199c..84712fe748 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; std::queue previous_commands_; // last two commands diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d21cac602d..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); - if (last_command_msg == nullptr) + if (last_command_msg_ == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(*last_command_msg_); + previous_commands_.emplace(*last_command_msg_); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( @@ -350,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..e29c6bbfee 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 010a890f52..1f9361dadd 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index ec7ca7bd5e..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,25 +95,27 @@ controller_interface::return_type TricycleController::update( } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); - if (last_command_msg == nullptr) + + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); + if (last_command_msg_ == nullptr) { RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by Limiters, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s @@ -271,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive); @@ -307,7 +309,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -397,7 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..5e868ebeea 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } From cd730558b2c449568c627f066cdf8bfa8bc4c3bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 15:06:45 +0100 Subject: [PATCH 36/36] Use the .hpp headers from `realtime_tools` package (#1406) --- .../include/admittance_controller/admittance_controller.hpp | 4 ++-- .../include/diff_drive_controller/diff_drive_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 2 +- .../forward_command_controller/forward_controllers_base.hpp | 2 +- .../include/gpio_controllers/gpio_command_controller.hpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 4 ++-- .../imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 2 +- .../joint_state_broadcaster/joint_state_broadcaster.hpp | 2 +- .../joint_trajectory_controller.hpp | 6 +++--- .../mecanum_drive_controller/mecanum_drive_controller.hpp | 4 ++-- .../include/mecanum_drive_controller/odometry.hpp | 4 ++-- .../parallel_gripper_action_controller.hpp | 4 ++-- pid_controller/include/pid_controller/pid_controller.hpp | 4 ++-- .../include/pose_broadcaster/pose_broadcaster.hpp | 2 +- .../range_sensor_broadcaster/range_sensor_broadcaster.hpp | 2 +- .../steering_controllers_library.hpp | 4 ++-- .../include/tricycle_controller/tricycle_controller.hpp | 4 ++-- 17 files changed, 29 insertions(+), 29 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..89cd195ce4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -32,8 +32,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 84712fe748..4fc4d37d4b 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -34,8 +34,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index e5a5349c32..2364dd7c8b 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -28,7 +28,7 @@ #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 4858e5ab64..fce7941d8a 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -23,7 +23,7 @@ #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 5149c74a1c..febac1294e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -26,8 +26,8 @@ #include "gpio_controllers/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" namespace gpio_controllers { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 38e0bd8191..478168391b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "gripper_controllers/hardware_interface_adapter.hpp" diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 1ba0b032ac..449020b6e2 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -26,7 +26,7 @@ // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.hpp" diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index ecc3c767f6..7ac98eccfb 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,7 +25,7 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "urdf/model.h" 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 2377b2c0b6..324ccfe4f1 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 @@ -36,9 +36,9 @@ #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 0fd7f6af29..f835752d43 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/mecanum_drive_controller_state.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index ac1ad060dd..241809e8c2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -16,8 +16,8 @@ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #define PLANAR_POINT_DIM 3 diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index d303990b89..739d1c570a 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "parallel_gripper_controller/visibility_control.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "parallel_gripper_action_controller_parameters.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index ce66fd06d4..0aba6cf849 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -29,8 +29,8 @@ #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" namespace pid_controller diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 621a90cc85..9317fce5e1 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -25,7 +25,7 @@ #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/pose_sensor.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 5a93f95982..2e4e47b018 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -25,7 +25,7 @@ #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/range_sensor.hpp" #include "sensor_msgs/msg/range.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 84a892d79e..49236986ee 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 @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 1f9361dadd..4d64549613 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp"