From 0a9f4b0c2b10c926d6c283520d763eefed0f06b5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 16 Nov 2023 23:17:20 +0000 Subject: [PATCH 01/41] Bump ros-tooling/action-ros-ci from 0.3.4 to 0.3.5 (#833) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 46922ccaac..76b2c22e53 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 723654b33e..1aaf30c639 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6a932a2dcd..72aa9af66c 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index a444a7f645..6b1af2b86c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From 25f2a14757e5624eabfd5804fb64b42100e72b52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 15:31:39 +0100 Subject: [PATCH 02/41] Resort overview page (#846) --- README.md | 2 +- doc/controllers_index.rst | 55 ++++++++++++---------- forward_command_controller/doc/userdoc.rst | 2 +- 3 files changed, 32 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index d41ec09b6c..4183b38fdc 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) -Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Navigation2. +Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. ## Build status diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index de33c3284c..9fceb90fd3 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -6,24 +6,9 @@ ros2_controllers ################# -`GitHub Repository `_ - - -Nomenclature -************ - -The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using. -The controllers are using `common hardware interface definitions`_. -The controllers' namespaces are commanding the following command interface types: - - - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` - - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` - - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` - - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - - ... - -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. +`Link to GitHub Repository `_ Guidelines and Best Practices @@ -36,34 +21,54 @@ Guidelines and Best Practices * -Available Controllers -********************* +Controllers for Mobile Robots +***************************** .. toctree:: :titlesonly: Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Admittance Controller <../admittance_controller/doc/userdoc.rst> Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> + +Controllers for Manipulators and Other Robots +********************************************* + +The controllers are using `common hardware interface definitions`_, and may use namespaces depending on the following command interface types: + + - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` + - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` + - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` + - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` + +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp + + +.. toctree:: + :titlesonly: + + Admittance Controller <../admittance_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> - Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> - Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> -Available Broadcasters +Broadcasters ********************** +Broadcasters are used to publish sensor data from hardware components to ROS topics. +In the sense of ros2_control, broadcasters are still controllers using the same controller interface as the other controllers above. + .. toctree:: :titlesonly: Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> - Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + 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> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 01aa2492a8..cd623a5acb 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ forward_command_controller ========================== -This is a base class implementing a feedforward controller. Specific implementations can be found in: +This is a base class implementing a feedforward controller. Specific implementations of this base class can be found in: * :ref:`position_controllers_userdoc` * :ref:`velocity_controllers_userdoc` From 41610fdf43b9875f079aa5933b3f95e9687e524d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:17:24 +0100 Subject: [PATCH 03/41] [JTC] Remove unused home pose (#845) --- .../joint_trajectory_controller.hpp | 2 -- .../src/joint_trajectory_controller.cpp | 28 ------------------- 2 files changed, 30 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 7542aa6951..397ccf347c 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 @@ -184,8 +184,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3936da155f..18f61f0e49 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -921,22 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); @@ -1028,8 +1013,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return CallbackReturn::SUCCESS; } @@ -1037,13 +1020,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { - // go home - if (traj_home_point_ptr_ != nullptr) - { - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_external_point_ptr_ = traj_home_point_ptr_; - } - return CallbackReturn::SUCCESS; } @@ -1070,11 +1046,7 @@ bool JointTrajectoryController::reset() } } - // iterator has no default value - // prev_traj_point_ptr_; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return true; } From d9a6bb43e312d8d74e52eddfb700e5249c18f4f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:18:46 +0100 Subject: [PATCH 04/41] [JTC] Fix dynamic reconfigure of tolerances (#849) * Fix dynamic reconfigure of tolerances * Fix static cast syntax --- .../src/joint_trajectory_controller.cpp | 5 +- .../test/test_trajectory_controller.cpp | 53 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 5 ++ 3 files changed, 61 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 18f61f0e49..dae7f13148 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -119,11 +119,12 @@ controller_interface::return_type JointTrajectoryController::update( if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - // use_closed_loop_pid_adapter_ is updated in on_configure only + default_tolerances_ = get_segment_tolerances(params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) { update_pids(); - default_tolerances_ = get_segment_tolerances(params_); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7842d50434..8b29c2cb16 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -449,6 +449,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) executor.cancel(); } +/** + * @brief check if dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.joint1.trajectory", 1.0), + rclcpp::Parameter("constraints.joint2.trajectory", 2.0), + rclcpp::Parameter("constraints.joint3.trajectory", 3.0), + rclcpp::Parameter("constraints.joint1.goal", 10.0), + rclcpp::Parameter("constraints.joint2.goal", 20.0), + rclcpp::Parameter("constraints.joint3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateController(); + + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 224265ad83..fdf4305cec 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -132,6 +132,11 @@ class TestableJointTrajectoryController std::vector get_pids() const { return pids_; } + joint_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const From 6d4fb2d9c5dc44751a19b277054ffb570406eebc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 22:33:01 +0100 Subject: [PATCH 05/41] fix tests for API break of passing controller manager update rate in init method (#854) --- .../test_ackermann_steering_controller.hpp | 2 +- .../test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_diff_drive_controller.cpp | 34 +++++++++---------- .../test_joint_group_effort_controller.cpp | 2 +- .../test_force_torque_sensor_broadcaster.cpp | 2 +- .../test/test_forward_command_controller.cpp | 2 +- ...i_interface_forward_command_controller.cpp | 2 +- .../test/test_gripper_controllers.cpp | 2 +- .../test/test_imu_sensor_broadcaster.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 2 +- .../test/test_trajectory_controller_utils.hpp | 2 +- .../test_joint_group_position_controller.cpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_controller.cpp | 14 ++++---- .../test_tricycle_steering_controller.hpp | 2 +- .../test_joint_group_velocity_controller.cpp | 2 +- 18 files changed, 40 insertions(+), 40 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 8af2f6bc94..a2849d5742 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index db708db6c5..19908d7f9f 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -185,7 +185,7 @@ 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, "", "", options); + auto result = controller_->init(controller_name, "", 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 34d6883a83..521506762b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 505aa44d2b..eb970d34a3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -195,7 +195,7 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -203,7 +203,7 @@ TEST_F(TestDiffDriveController, configure_fails_without_parameters) TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -223,7 +223,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -239,7 +239,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -259,7 +259,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -292,7 +292,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -327,7 +327,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -363,7 +363,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -398,7 +398,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -435,7 +435,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -469,7 +469,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -483,7 +483,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -499,7 +499,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -516,7 +516,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -533,7 +533,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -550,7 +550,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -599,7 +599,7 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 23555c578f..f9d72ab202 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,7 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", ""); + const auto result = controller_->init("test_joint_group_effort_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; 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 82f677410e..bfe7561642 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 @@ -53,7 +53,7 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", ""); + const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 2ab507ef29..62d5512b4e 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -60,7 +60,7 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", ""); + const auto result = controller_->init("forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 00ca4ae2d1..7826c85355 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -62,7 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", ""); + const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 572e755743..0fc10c0c73 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -60,7 +60,7 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", ""); + const auto result = controller_->init("gripper_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0b782efc5f..0fb987ce77 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -53,7 +53,7 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", ""); + const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index a6c0708fd9..3a74f599b4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,7 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init("joint_state_broadcaster", ""); + const auto result = state_broadcaster_->init("joint_state_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index fdf4305cec..3a7e7e5cf1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -196,7 +196,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", "", node_options); + auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); if (ret != controller_interface::return_type::OK) { FAIL(); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 07f42e4eba..3b4f00be12 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,7 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", ""); + const auto result = controller_->init("test_joint_group_position_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ab937146cb..a857141ea9 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -34,7 +34,7 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init(broadcaster_name, ""); + result = range_broadcaster_->init(broadcaster_name, "", 0); if (controller_interface::return_type::OK == result) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 226d4f4291..5caf347ac1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 054fff10ce..504ca381ce 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -173,14 +173,14 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -200,7 +200,7 @@ TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -213,7 +213,7 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -227,7 +227,7 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -243,7 +243,7 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -292,7 +292,7 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e9d1023d98..c5f6985d4e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 185c630bc9..4cbf1b7342 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,7 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", ""); + const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From ba7405443b1cdb78c0e0f3ec458738da37d690d0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:35 +0000 Subject: [PATCH 06/41] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 6 ++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 7 +++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 6 ++++++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 6 ++++++ 20 files changed, 119 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index d679a782d4..7b9a59976e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ba5a131094..1a359ce855 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 7b14b2d617..f18f0188a7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b3f4b127e3..6862ea574c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) +* [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota, Tony Baltovski + 3.17.0 (2023-10-31) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index baaada7c22..15c3edef1b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9b6dea8ef9..8bc4a07db2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4bc9d41c74..77f249ccbd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Resort overview page (`#846 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b7c7242520..a59b2c24ce 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 8644b008a0..d1fdff1378 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index fd56b72c3b..456e81090b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9eece3af21..66d24ea513 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) +* [JTC] Remove unused home pose (`#845 `_) +* [JTC] Activate update of dynamic parameters (`#761 `_) +* [JTC] Fix tests when state offset is used (`#797 `_) +* [JTC] Remove deprecation warnings, set `allow_nonzero_velocity_at_trajectory_end` default false (`#834 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota, Dr Denis + 3.17.0 (2023-10-31) ------------------- * Cleanup comments and unnecessary checks (`#803 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5239c43407..b214480e86 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b1084b4a24..1f04cf47e7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 187db3d351..873dd5b55f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index acca28227f..0d1e8ad2ef 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 +----------- + 3.17.0 (2023-10-31) ------------------- * [TestNodes] Optimize output about setup of JTC publisher (`#792 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index e723d39c6f..7263a948d5 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 +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8082dc9070..5279d003cf 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Steering controllers library: fix open loop mode (`#793 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bda1c89d32..605a4f896d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a7e6854ea8..a41ee0623c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 358ee9f499..9d4102e69a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- From 0d3fc520831d10d4c1d16784e7378c2a0bf7b10f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:59 +0000 Subject: [PATCH 07/41] 4.0.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7b9a59976e..7a94360e4b 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 656c88feae..421f098f96 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.17.0 + 4.0.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 1a359ce855..b94892ac83 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 62cf127d9f..a5f7e7e353 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.17.0 + 4.0.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 f18f0188a7..278921e18b 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 9ee4c53049..e3063672d6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.17.0 + 4.0.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 6862ea574c..5f57b7e833 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) * [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 01dc69413a..56b9552f93 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.17.0 + 4.0.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 15c3edef1b..6c7d7d6fa3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d0627bea10..ef2610d34f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.17.0 + 4.0.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 8bc4a07db2..17a6d4f280 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 81c865ddfb..4a1df9ff11 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.17.0 + 4.0.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 77f249ccbd..941b849edd 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Resort overview page (`#846 `_) * Adjust tests after passing URDF to controllers (`#817 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 246bc6777d..0fe44110b1 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a59b2c24ce..f35dcd1a32 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 391bd1bb7d..08b0298e1f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.17.0 + 4.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d1fdff1378..4d2d2d01b3 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 2dcacc1f0f..1819b974e2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.17.0 + 4.0.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 456e81090b..ce89b01912 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e4fecb14d7..1364b1a164 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.17.0 + 4.0.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 66d24ea513..a4486e4e98 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) * [JTC] Remove unused home pose (`#845 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f4efc2c060..fe10d9583d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.17.0 + 4.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b214480e86..26462d51a9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b3cd9b6cfb..2112f7d8c5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.17.0 + 4.0.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 1f04cf47e7..71ee9a7119 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index cf6545d65e..3ece4bda4b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.17.0 + 4.0.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 873dd5b55f..2c2e5ec358 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 49c87b152d..6a213308c5 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.17.0 + 4.0.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 0d1e8ad2ef..e8eaf9c382 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.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index ef8c7b572d..5698930bff 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.17.0 + 4.0.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 102c39b607..a129b2d0a7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.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 7263a948d5..0c08780904 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.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 08f885aef3..41cf5e85d7 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 - 3.17.0 + 4.0.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 c0f4a96c2a..5a5b979d41 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.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 5279d003cf..1a76469d6c 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 0194cfd041..932fda9a35 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.17.0 + 4.0.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 605a4f896d..6bc3d26f53 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9a6cf3ef2d..a663dcacde 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.17.0 + 4.0.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 a41ee0623c..0ade9bca61 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.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e39971982c..7f9d14c2fe 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.17.0 + 4.0.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 9d4102e69a..bb7e231222 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f336606b26..6ce13e6adf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2854a0bc5da016b6ddc55383e6f7bd7b8e1e57ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 26 Nov 2023 22:06:17 +0100 Subject: [PATCH 08/41] Use testing and remove wrong field of workflow_dispatch (#860) --- .github/workflows/humble-abi-compatibility.yml | 4 +--- .github/workflows/iron-abi-compatibility.yml | 4 +--- .github/workflows/rolling-abi-compatibility.yml | 4 +--- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 708ea5c1f4..5c288fabfb 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index 20d93f5af1..ab6642625f 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Iron - ABI Compatibility Check on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: iron - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 3911434a23..73055ef3e5 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Rolling - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - master @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: rolling - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true From b8532284853c990a83741b85983f18ed38ef5f89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:31:10 +0100 Subject: [PATCH 09/41] [JTC] Improve update methods for tests (#858) --- .../test/test_trajectory_actions.cpp | 16 +- .../test/test_trajectory_controller.cpp | 967 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 143 ++- 3 files changed, 565 insertions(+), 561 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3b80abcb9b..ed13a24485 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -242,7 +242,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -298,7 +298,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -350,7 +350,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -409,7 +409,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -460,7 +460,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -509,7 +509,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -555,7 +555,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -603,7 +603,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position, // i.e., active but trivial trajectory (one point only) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8b29c2cb16..7c026d5e7a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" @@ -103,77 +101,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter different than joint_names_ 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(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -316,6 +257,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; @@ -422,7 +368,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); auto pids = traj_controller_->get_pids(); if (traj_controller_->use_closed_loop_pid_adapter()) @@ -433,7 +379,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) double kp = 1.0; SetPidParameters(kp); - updateController(); + updateControllerAsync(); pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); @@ -458,7 +404,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -486,7 +432,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -513,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup without start_with_holding being set, we expect no active trajectory ASSERT_FALSE(traj_controller_->has_active_traj()); @@ -531,7 +477,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup with start_with_holding being set, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup @@ -552,7 +498,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -568,84 +513,206 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + 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], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + 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_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are angle_wraparound if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] angle_wraparound? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + 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] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + 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_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // no normalization of position error + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); @@ -685,13 +752,13 @@ TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) /** * @brief check if no timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, no_timeout) { rclcpp::executors::MultiThreadedExecutor executor; // zero is default value, just for demonstration rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -707,28 +774,27 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); // is the trajectory still active? EXPECT_TRUE(traj_controller_->has_active_traj()); // should still hold the points from above EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); - EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); // value of velocities is different from above due to spline interpolation - EXPECT_GT(state_msg->reference.velocities[0], 0.0); - EXPECT_GT(state_msg->reference.velocities[1], 0.0); - EXPECT_GT(state_msg->reference.velocities[2], 0.0); + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); executor.cancel(); } @@ -736,6 +802,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) /** * @brief check if timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, timeout) { rclcpp::executors::MultiThreadedExecutor executor; @@ -743,7 +810,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); double kp = 1.0; // activate feedback control for testing velocity/effort PID SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); - subscribeToState(); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -768,11 +834,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -792,130 +853,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -/** - * @brief check if position error of revolute joints are angle_wraparound if configured so - */ -TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) -{ - rclcpp::executors::MultiThreadedExecutor executor; - constexpr double k_p = 10.0; - std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); - subscribeToState(); - - size_t n_joints = joint_names_.size(); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - - const auto allowed_delta = 0.1; - - // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); - - // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); - - // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - - // is error.positions[2] angle_wraparound? - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], - state_msg->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], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - } - } - - if (traj_controller_->has_effort_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } - } - - executor.cancel(); -} - /** * @brief check if use_closed_loop_pid is active */ @@ -944,7 +881,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -960,35 +896,34 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); - EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); - EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); } if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); } // no change in state interface should happen if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -998,9 +933,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations - EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); - EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); - EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); } executor.cancel(); @@ -1016,6 +951,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ @@ -1025,29 +961,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(3); traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); - for (size_t i = 0; i < 3; i++) + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) { - // factor 2 comes from the linear interpolation in the spline trajectory for constant - // acceleration - traj_msg.points[0].velocities[i] = - 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, - // otherwise acceleration is zero from the spline trajectory) - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1068,19 +1002,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_acceleration_command_interface()) { - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); } if (traj_controller_->has_effort_command_interface()) @@ -1106,37 +1030,42 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(2); traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1148,7 +1077,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1156,24 +1085,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) - << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) - << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - } - EXPECT_NEAR(0.0, joint_acc_[2], threshold) + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1185,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1227,47 +1147,45 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe traj_controller_->wait_for_trajectory(executor); // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1408,8 +1326,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); - std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; @@ -1424,8 +1340,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1440,7 +1356,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities[1] = 0.0; expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1450,7 +1367,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1464,7 +1380,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) 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 - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time @@ -1473,14 +1390,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1493,19 +1410,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory 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 - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = - rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); // it should not have accepted the new goal but finish the old one expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1513,7 +1431,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1538,7 +1455,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); @@ -1551,28 +1469,21 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } -// TODO(destogl) this test fails with errors -// second publish() gives an error, because end time is before current time -// as well as -// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, -// which exceeds COMMON_THRESHOLD, where -// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, -// 2: joint_pos_[0] evaluates to 6.2700020099999998, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_jump_when_state_tracking_error_updated/0, where GetParam() = -// ({ "position" }, { "position" }) (3372 ms) - -#if 0 TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1585,95 +1496,95 @@ 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); std::vector> points{{first_goal}}; - publish(time_from_start, points, - rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, - rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // 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)); + // 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] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 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); + 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); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // 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)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif - -// TODO(destogl) this test fails -// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, -// which exceeds COMMON_THRESHOLD, where -// 2: second_goal[0] evaluates to 6.5999999999999996, -// 2: joint_pos_[0] evaluates to 6.5670133649999993, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = -// ({ "position" }, { "position", "velocity" }) (3374 ms) -#if 0 + TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { 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); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1684,77 +1595,79 @@ 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); 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); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // 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)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // 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); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif // Testing that values are read from state interfaces when hardware is started for the first // time and hardware state has offset --> this is indicated by NaN values in command interfaces @@ -1902,7 +1815,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1934,7 +1847,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1942,7 +1855,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectHoldingPoint(hold_position); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3a7e7e5cf1..3c8e252067 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -29,7 +29,7 @@ namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -151,6 +151,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -233,21 +237,24 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor); - - // add this to simplify tests, can be overwritten by parameters - rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); - traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); - - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); - - // set optional parameters - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), + [](const rclcpp::Parameter & param) { + return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; + }) != parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -407,43 +414,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() < end_time) + while (clock.now() <= end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; } - void waitAndCompareState( + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -453,10 +502,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR( - expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getState() const @@ -573,6 +623,47 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_; From fe0c91d4f36939866276d63c1d8f552544f6818f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:32:27 +0100 Subject: [PATCH 10/41] joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (#859) --- .../test/test_joint_state_broadcaster.cpp | 21 +++++++++---------- .../test/test_joint_state_broadcaster.hpp | 3 +++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3a74f599b4..b44152a453 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -531,19 +531,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) state_broadcaster_->get_node()->set_parameter( {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - // configure ok - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message("joint_states", joint_state_msg); const size_t NUM_JOINTS = JOINT_NAMES.size(); // 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_EQ(joint_state_msg.position[0], custom_joint_value_); @@ -585,7 +578,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) controller_interface::return_type::OK); } -void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +void JointStateBroadcasterTest::activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) { auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -616,9 +610,14 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st "controller/broadcaster update loop"; // take message from subscription - sensor_msgs::msg::JointState joint_state_msg; rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); +} + +void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +{ + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message(topic, joint_state_msg); const size_t NUM_JOINTS = joint_names_.size(); ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..fcaa40e8d5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -71,6 +71,9 @@ class JointStateBroadcasterTest : public ::testing::Test void test_published_dynamic_joint_state_message(const std::string & topic); + void activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & msg); + protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; From b6841ead459f6efb76153e8474a3f5443d48d2b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:54:41 +0100 Subject: [PATCH 11/41] Increase test coverage of interface configuration getters (#856) --- .../test_force_torque_sensor_broadcaster.cpp | 25 +++++++- .../test_force_torque_sensor_broadcaster.hpp | 2 +- .../test/test_forward_command_controller.cpp | 42 +++++++++++++ ...i_interface_forward_command_controller.cpp | 23 +++++++ .../test/test_gripper_controllers.cpp | 12 ++++ .../test/test_imu_sensor_broadcaster.cpp | 23 +++++++ .../test/test_joint_state_broadcaster.cpp | 62 ++++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 4 +- .../test/test_range_sensor_broadcaster.cpp | 27 +++++++- .../test/test_tricycle_controller.cpp | 15 +++++ 10 files changed, 228 insertions(+), 7 deletions(-) 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 bfe7561642..ce6a04ec1f 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 @@ -32,6 +32,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -157,6 +159,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -175,7 +183,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { SetUpFTSBroadcaster(); @@ -186,6 +194,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + + // deactivate passed + ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) 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 5477b3fa6f..fe5b0ab3ba 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 @@ -36,7 +36,7 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty); - FRIEND_TEST(ForceTorqueSensorBroadcasterTest, ActivateSuccess); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); }; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 62d5512b4e..236cb14edd 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -34,6 +34,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -128,6 +130,13 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -173,9 +182,23 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -323,9 +346,22 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -344,6 +380,12 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7826c85355..2d3b61e211 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -36,6 +36,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -148,6 +150,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -282,6 +291,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes { SetUpController(true); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // send command auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; @@ -300,6 +316,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 0fc10c0c73..da9e15840e 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -31,6 +31,8 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; template void GripperControllerTest::SetUpTestCase() @@ -108,6 +110,16 @@ TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) ASSERT_EQ( this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0fb987ce77..83edc044d3 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -113,6 +115,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -126,6 +134,21 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + + // deactivate passed + ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index b44152a453..3e45740bbc 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -163,7 +163,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); } -TEST_F(JointStateBroadcasterTest, ActivateTest) +TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); @@ -177,6 +177,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -218,6 +224,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -259,6 +271,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -287,7 +305,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ElementsAreArray(interface_names_)); } -TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) +TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) { const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0]}; @@ -300,6 +318,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -329,6 +353,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); + + // deactivate + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT( + state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -344,6 +378,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -412,6 +452,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -455,6 +501,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, SizeIs(0)); @@ -492,6 +544,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) 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()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(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 fcaa40e8d5..fa9d29c936 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -33,10 +33,10 @@ using hardware_interface::HW_IF_VELOCITY; class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface); + FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a857141ea9..7b8e6d0e02 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -22,6 +22,9 @@ #include "hardware_interface/loaned_state_interface.hpp" +using testing::IsEmpty; +using testing::SizeIs; + void RangeSensorBroadcasterTest::SetUp() { // initialize controller @@ -130,9 +133,15 @@ TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); } -TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) +TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) { init_broadcaster("test_range_sensor_broadcaster"); @@ -141,6 +150,22 @@ TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + + ASSERT_EQ( + range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 504ca381ce..d8beedae42 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -39,6 +39,7 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +using testing::UnorderedElementsAre; class TestableTricycleController : public tricycle_controller::TricycleController { @@ -209,6 +210,20 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + state_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) From 35556854883b546143d7773c43053650a8b23840 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 30 Nov 2023 16:26:33 +0100 Subject: [PATCH 12/41] added documentation on common controller parameters (#855) --- doc/controllers_index.rst | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 9fceb90fd3..5f8780d961 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -72,3 +72,12 @@ 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> + + +Common Controller Parameters +**************************** + +Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents `__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager. + +* ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager. +* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. From fcc0847958c17a942e2b233d3bea1ab2d2ac05dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 30 Nov 2023 17:36:36 +0100 Subject: [PATCH 13/41] [JTC] Activate checks for parameter validation (#857) --- .../test/test_trajectory_controller.cpp | 157 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 22 ++- 2 files changed, 91 insertions(+), 88 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7c026d5e7a..0b6651e79f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - traj_controller_->get_node()->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) ASSERT_EQ( state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); - ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ActivateTrajectoryController(); - - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -245,8 +242,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(false, deactivated_positions); - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(false, deactivated_positions); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // it should still be holding the position at time of deactivation @@ -1918,72 +1914,73 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); -// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` -// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -// { -// auto set_parameter_and_check_result = [&]() -// { -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// SetParameters(); // This call is replacing the way parameters are set via launch -// traj_controller_->get_node()->configure(); -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// }; -// -// SetUpTrajectoryController(false); -// -// // command interfaces: empty -// command_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // command interfaces: bad_name -// command_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // command interfaces: effort has to be only -// command_interface_types_ = {"effort", "position"}; -// set_parameter_and_check_result(); -// -// // command interfaces: velocity - position not present -// command_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // command interfaces: acceleration without position and velocity -// command_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: empty -// state_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // state interfaces: cannot not be effort -// state_interface_types_ = {"effort"}; -// set_parameter_and_check_result(); -// -// // state interfaces: bad name -// state_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // state interfaces: velocity - position not present -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: acceleration without position and velocity -// state_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // velocity-only command interface: position - velocity not present -// command_interface_types_ = {"velocity"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// -// // effort-only command interface: position - velocity not present -// command_interface_types_ = {"effort"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// } +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->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 3c8e252067..b530517517 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}) + { + auto ret = SetUpTrajectoryControllerLocal(parameters); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + } + + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); @@ -200,12 +211,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); - if (ret != controller_interface::return_type::OK) - { - FAIL(); - } - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + return traj_controller_->init(controller_name_, "", 0, "", node_options); } void SetPidParameters( @@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test initial_eff_joints); } - void ActivateTrajectoryController( + rclcpp_lifecycle::State ActivateTrajectoryController( bool separate_cmd_and_state_values = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, @@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_node()->activate(); + return traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } From 62ce4879e45fdec2f462f14a695cf579afbfe866 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:39:06 +0100 Subject: [PATCH 14/41] [JTC] Remove start_with_holding option (#839) --- .../src/joint_trajectory_controller.cpp | 7 ++---- ...oint_trajectory_controller_parameters.yaml | 5 ---- .../test/test_trajectory_controller.cpp | 23 ++----------------- 3 files changed, 4 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dae7f13148..d24fd5a34a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -944,11 +944,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } - // Should the controller start by holding position at the beginning of active state? - if (params_.start_with_holding) - { - add_new_trajectory_msg(set_hold_position()); - } + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 87ca1daea5..4981489d36 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -73,11 +73,6 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } - start_with_holding: { - type: bool, - default_value: true, - description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", - } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b6651e79f..0b049000e3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -444,24 +444,6 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) executor.cancel(); } -/** - * @brief check if hold on startup is deactivated - */ -TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) -{ - rclcpp::executors::MultiThreadedExecutor executor; - - rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); - - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup without start_with_holding being set, we expect no active trajectory - ASSERT_FALSE(traj_controller_->has_active_traj()); - - executor.cancel(); -} - /** * @brief check if hold on startup */ @@ -469,12 +451,11 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) { rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup with start_with_holding being set, we expect an active trajectory: + // after startup, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; From e4f1700ef653268130e25e65e351dc4d5572b54d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:40:56 +0100 Subject: [PATCH 15/41] [JTC] Continue with last trajectory-point on success (#842) --- joint_trajectory_controller/doc/userdoc.rst | 4 +- .../joint_trajectory_controller.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 16 ++- .../test/test_trajectory_actions.cpp | 136 +++++++++++++----- .../test/test_trajectory_controller.cpp | 14 +- .../test/test_trajectory_controller_utils.hpp | 56 +++----- 6 files changed, 162 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 58ba734b45..6a34185437 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -150,12 +150,14 @@ Actions [#f1]_ /follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller - The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. + Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. + .. _Subscriber: Subscriber [#f1]_ 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 397ccf347c..d16e4f8267 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 @@ -174,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Timeout to consider commands old double cmd_timeout_; - // Are we holding position? + // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; @@ -244,9 +244,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d24fd5a34a..9306b07354 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -362,7 +362,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -1466,6 +1466,20 @@ JointTrajectoryController::set_hold_position() return hold_position_msg_ptr_; } +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + bool JointTrajectoryController::contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ed13a24485..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0) + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - point.positions = point_positions; points.push_back(point); @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(point_positions); - } - else + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions, point_velocities); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - point1.positions.resize(joint_names_.size()); - point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); - point2.positions.resize(joint_names_.size()); - point2.positions = points_positions.at(1); points.push_back(point2); @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(points_positions.at(1)); - } - else + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } /** @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(0)); + expectCommandPoint(points_positions.at(0)); } /** @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + expectCommandPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) // it should be holding the last position, // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(cancelled_position); + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b049000e3..05010c562c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // it should still be holding the position at time of deactivation // i.e., active but trivial trajectory (one point only) traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - expectHoldingPoint(deactivated_positions); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -459,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(initial_positions); executor.cancel(); } @@ -819,12 +819,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // should hold last position with zero velocity if (traj_controller_->has_position_command_interface()) { - expectHoldingPoint(points.at(2)); + expectCommandPoint(points.at(2)); } else { // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + expectCommandPoint(INITIAL_POS_JOINTS); } executor.cancel(); @@ -1795,7 +1795,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -1827,14 +1827,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point - expectHoldingPoint(hold_position); + expectCommandPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b530517517..4a65b4ad51 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -521,7 +521,8 @@ class TrajectoryControllerTest : public ::testing::Test return state_msg_; } - void expectHoldingPoint(std::vector point) + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -531,16 +532,16 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); } if (traj_controller_->has_acceleration_command_interface()) @@ -557,40 +558,29 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(0.0, joint_eff_[2]); } } - else + else // traj_controller_->use_closed_loop_pid_adapter() == true { // velocity or effort PID? - // velocity setpoint is always zero -> feedforward term does not have an effect // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", velocity command is " << joint_vel_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", velocity command is " << joint_vel_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", velocity command is " << joint_vel_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", effort command is " << joint_eff_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", effort command is " << joint_eff_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", effort command is " << joint_eff_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } } } } From 757310073cadb20e551c407354f5a40e09e39a2a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:13 +0000 Subject: [PATCH 16/41] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ 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 | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 20 files changed, 80 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7a94360e4b..ebd6d5ef43 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.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b94892ac83..19fbe5864d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 278921e18b..dbfe611d52 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.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5f57b7e833..e0166e9dd8 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.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6c7d7d6fa3..963c8916db 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 17a6d4f280..005b4ba6ac 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 +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 941b849edd..f65fefd016 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f35dcd1a32..3e06cc1830 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d2d2d01b3..b05b8ce192 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ce89b01912..9f80626bb2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a4486e4e98..1ad5b44a48 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] Continue with last trajectory-point on success (`#842 `_) +* [JTC] Remove start_with_holding option (`#839 `_) +* [JTC] Activate checks for parameter validation (`#857 `_) +* [JTC] Improve update methods for tests (`#858 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 26462d51a9..caf8f3925c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 71ee9a7119..d15eb127e6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2c2e5ec358..9ae67679d7 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e8eaf9c382..c3f8302d06 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.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0c08780904..58694e910d 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.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 1a76469d6c..b8f6854520 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.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 6bc3d26f53..9e67804591 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 0ade9bca61..bd2fc2d678 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.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bb7e231222..cb05fb45f1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) From 23f944f26e7e21764be3acdb6610cd59866c9bfc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:35 +0000 Subject: [PATCH 17/41] 4.1.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index ebd6d5ef43..a1cbe9596e 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 421f098f96..476c4d3050 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.0.0 + 4.1.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 19fbe5864d..615a457992 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a5f7e7e353..da5b652bd0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.0.0 + 4.1.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 dbfe611d52..bee3606ec8 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index e3063672d6..c627577ee8 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.0.0 + 4.1.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 e0166e9dd8..8ee42d701f 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 56b9552f93..03dffe764d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.0.0 + 4.1.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 963c8916db..f4496d3927 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ef2610d34f..e55e095c57 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.0.0 + 4.1.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 005b4ba6ac..b87aaa2641 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.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4a1df9ff11..48db0e49b6 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.0.0 + 4.1.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 f65fefd016..b38f021587 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.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0fe44110b1..c500c1f714 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3e06cc1830..798a0f3e67 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 08b0298e1f..34c7ba663e 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.0.0 + 4.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b05b8ce192..2c311626cf 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.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 1819b974e2..3ab9c5fff6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.0.0 + 4.1.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 9f80626bb2..f2295beca3 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.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1364b1a164..37ba7b4912 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.0.0 + 4.1.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 1ad5b44a48..7e4c25a474 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.1.0 (2023-12-01) +------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) * [JTC] Remove start_with_holding option (`#839 `_) * [JTC] Activate checks for parameter validation (`#857 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fe10d9583d..b41a7cad0c 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.0.0 + 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index caf8f3925c..6f0870c9ae 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2112f7d8c5..1aec89e59d 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.0.0 + 4.1.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 d15eb127e6..be35427e46 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.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3ece4bda4b..222d87e3bb 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.0.0 + 4.1.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 9ae67679d7..46f994c792 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6a213308c5..d34a5b1375 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.0.0 + 4.1.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 c3f8302d06..a7a2f7a1fd 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 5698930bff..16dd709c25 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.0.0 + 4.1.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 a129b2d0a7..baa01b97c2 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.0.0", + version="4.1.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 58694e910d..ee80d4b30d 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 41cf5e85d7..24ed2c9d63 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.0.0 + 4.1.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 5a5b979d41..2d855d9255 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.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 b8f6854520..bae8e6e557 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 932fda9a35..5de0ddc872 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.0.0 + 4.1.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 9e67804591..0203298259 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index a663dcacde..5f382bf8d2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.0.0 + 4.1.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 bd2fc2d678..4abf0788fb 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.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7f9d14c2fe..f565ef0c02 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.0.0 + 4.1.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 cb05fb45f1..91575e6075 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 6ce13e6adf..d7376e8e6b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 038a54794ef06147e7a4ab5298b8c9eabe65e287 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Dec 2023 20:01:15 +0100 Subject: [PATCH 18/41] =?UTF-8?q?=F0=9F=9A=80=20Add=20PID=20controller=20?= =?UTF-8?q?=F0=9F=8E=89=20(#434)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/controllers_index.rst | 1 + pid_controller/CMakeLists.txt | 108 ++++ pid_controller/README.md | 7 + pid_controller/doc/userdoc.rst | 86 +++ .../include/pid_controller/pid_controller.hpp | 141 +++++ .../pid_controller/visibility_control.h | 49 ++ pid_controller/package.xml | 35 ++ pid_controller/pid_controller.xml | 8 + pid_controller/src/pid_controller.cpp | 521 +++++++++++++++++ pid_controller/src/pid_controller.yaml | 87 +++ .../test/pid_controller_params.yaml | 11 + .../test/pid_controller_preceding_params.yaml | 11 + .../test/test_load_pid_controller.cpp | 43 ++ pid_controller/test/test_pid_controller.cpp | 531 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 285 ++++++++++ .../test/test_pid_controller_preceding.cpp | 103 ++++ ros2_controllers/package.xml | 1 + 17 files changed, 2028 insertions(+) create mode 100644 pid_controller/CMakeLists.txt create mode 100644 pid_controller/README.md create mode 100644 pid_controller/doc/userdoc.rst create mode 100644 pid_controller/include/pid_controller/pid_controller.hpp create mode 100644 pid_controller/include/pid_controller/visibility_control.h create mode 100644 pid_controller/package.xml create mode 100644 pid_controller/pid_controller.xml create mode 100644 pid_controller/src/pid_controller.cpp create mode 100644 pid_controller/src/pid_controller.yaml create mode 100644 pid_controller/test/pid_controller_params.yaml create mode 100644 pid_controller/test/pid_controller_preceding_params.yaml create mode 100644 pid_controller/test/test_load_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.hpp create mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5f8780d961..d042fe79dd 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..81cbe6f006 --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +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(pid_controller_parameters + src/pid_controller.yaml +) + +add_library(pid_controller SHARED + src/pid_controller.cpp +) +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC + $ + $ +) +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters +) +ament_target_dependencies(pid_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(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_pid_controller + test/test_pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller PRIVATE include) + target_link_libraries(test_pid_controller pid_controller) + ament_target_dependencies( + test_pid_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_preceding + test/test_pid_controller_preceding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml + ) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) + ament_target_dependencies( + test_pid_controller_preceding + controller_interface + hardware_interface + ) + + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) + target_include_directories(test_load_pid_controller PRIVATE include) + ament_target_dependencies( + test_load_pid_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/pid_controller +) + +install(TARGETS + pid_controller + pid_controller_parameters + EXPORT export_pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..01e8fddac8 --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implementation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5570f523fe --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,86 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst + +.. _pid_controller_userdoc: + +PID Controller +-------------------------------- + +PID Controller implementation that uses PidROS implementation from `control_toolbox `_ package. +The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. + +Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example: + +- reference/state POSITION; command VELOCITY --> PI CONTROLLER +- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER + +- reference/state VELOCITY; command POSITION --> PD CONTROLLER +- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER + +- reference/state POSITION; command POSITION --> PID CONTROLLER +- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER +- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER +- reference/state EFFORT; command EFFORT --> PID CONTROLLER + +.. note:: + + Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)` for the same purpose by sending only one reference point into it. + Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. + +Execution logic of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics. +If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + +Using the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pluginlib-Library: pid_controller +Plugin name: pid_controller/PidController + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +If controller is not in chained mode (``in_chained_mode == false``): + +- /reference [control_msgs/msg/MultiDOFCommand] + +If controller parameter ``use_external_measured_states`` is true: + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +,,,,,,,,,,, + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +The PID controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/pid_controller.yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp new file mode 100644 index 0000000000..a34dc9f87f --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,141 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/multi_dof_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "pid_controller/visibility_control.h" +#include "pid_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace pid_controller +{ + +enum class feedforward_mode_type : std::uint8_t +{ + OFF = 0, + ON = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + PID_CONTROLLER__VISIBILITY_PUBLIC + PidController(); + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + +protected: + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector reference_and_state_dof_names_; + size_t dof_; + std::vector measured_state_values_; + + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector feedforward_gain_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> measured_state_; + + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // internal methods + void update_parameters(); + controller_interface::CallbackReturn configure_parameters(); + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h new file mode 100644 index 0000000000..1509364b5a --- /dev/null +++ b/pid_controller/include/pid_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 PID_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_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 PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/package.xml b/pid_controller/package.xml new file mode 100644 index 0000000000..7d52121582 --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,35 @@ + + + + pid_controller + 0.0.0 + Controller based on PID implememenation from control_toolbox package. + Bence Magyar + Denis Štogl + Denis Štogl + Apache-2.0 + + ament_cmake + + generate_parameter_library + + angles + control_msgs + control_toolbox + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/pid_controller/pid_controller.xml b/pid_controller/pid_controller.xml new file mode 100644 index 0000000000..5b90741ae6 --- /dev/null +++ b/pid_controller/pid_controller.xml @@ -0,0 +1,8 @@ + + + + PidController ros2_control controller. + + + diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp new file mode 100644 index 0000000000..373e941d96 --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,521 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); + +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + msg->dof_names = dof_names; + msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); +} + +void reset_controller_measured_state_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + reset_controller_reference_msg(msg, dof_names); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(feedforward_mode_type::OFF); + + 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; +} + +void PidController::update_parameters() +{ + if (!param_listener_->is_old(params_)) + { + return; + } + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn PidController::configure_parameters() +{ + update_parameters(); + + if (!params_.reference_and_state_dof_names.empty()) + { + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; + } + else + { + reference_and_state_dof_names_ = params_.dof_names; + } + + if (params_.dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'dof_names' (%zu) and 'reference_and_state_dof_names' (%zu) parameters has to be " + "the same!", + params_.dof_names.size(), reference_and_state_dof_names_.size()); + return CallbackReturn::FAILURE; + } + + dof_ = params_.dof_names.size(); + + // TODO(destogl): is this even possible? Test it... + if (params_.gains.dof_names_map.size() != dof_) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", + params_.gains.dof_names_map.size(), dof_); + return CallbackReturn::FAILURE; + } + + pids_.resize(dof_); + + for (size_t i = 0; i < dof_; ++i) + { + // prefix should be interpreted as parameters prefix + pids_[i] = + std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_and_state_dof_names_.clear(); + pids_.clear(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&PidController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + input_ref_.writeFromNonRT(msg); + + // input state Subscriber and callback + if (params_.use_external_measured_states) + { + auto measured_state_callback = + [&](const std::shared_ptr msg) -> void + { + // TODO(destogl): Sort the input values based on joint and interface names + measured_state_.writeFromNonRT(msg); + }; + measured_state_subscriber_ = get_node()->create_subscription( + "~/measured_state", subscribers_qos, measured_state_callback); + } + std::shared_ptr measured_state_msg = + std::make_shared(); + reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); + measured_state_.writeFromNonRT(measured_state_msg); + + measured_state_values_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + auto set_feedforward_control_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + response->success = true; + }; + + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); + + try + { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(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; + } + + // Reserve memory in state publisher + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) + { + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::reference_callback(const std::shared_ptr msg) +{ + if (msg->dof_names.empty() && msg->values.size() == reference_and_state_dof_names_.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Reference massage does not have DoF names defined. " + "Assuming that value have order as defined state DoFs"); + auto ref_msg = msg; + ref_msg->dof_names = reference_and_state_dof_names_; + input_ref_.writeFromNonRT(ref_msg); + } + else if ( + msg->dof_names.size() == reference_and_state_dof_names_.size() && + msg->values.size() == reference_and_state_dof_names_.size()) + { + auto ref_msg = msg; // simple initialization + + // sort values in the ref_msg + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + + bool all_found = true; + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + auto found_it = + std::find(ref_msg->dof_names.begin(), ref_msg->dof_names.end(), msg->dof_names[i]); + if (found_it == msg->dof_names.end()) + { + all_found = false; + RCLCPP_WARN( + get_node()->get_logger(), "DoF name '%s' not found in the defined list of state DoFs.", + msg->dof_names[i].c_str()); + break; + } + + auto position = std::distance(ref_msg->dof_names.begin(), found_it); + ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; + } + + if (all_found) + { + input_ref_.writeFromNonRT(ref_msg); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) and/or values (%zu) is not matching the expected size (%zu).", + msg->dof_names.size(), msg->values.size(), reference_and_state_dof_names_.size()); + } +} + +controller_interface::InterfaceConfiguration PidController::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(params_.dof_names.size()); + for (const auto & dof_name : params_.dof_names) + { + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (params_.use_external_measured_states) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(dof_name + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + ++index; + } + } + + return reference_interfaces; +} + +bool PidController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn PidController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command (the same number as state interfaces) + reset_controller_reference_msg(*(input_ref_.readFromRT()), reference_and_state_dof_names_); + reset_controller_measured_state_msg( + *(measured_state_.readFromRT()), reference_and_state_dof_names_); + + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto current_ref = input_ref_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) + { + if (!std::isnan((*current_ref)->values[i])) + { + reference_interfaces_[i] = (*current_ref)->values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + { + reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + } + + (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type PidController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // check for any parameter updates + update_parameters(); + + if (params_.use_external_measured_states) + { + const auto measured_state = *(measured_state_.readFromRT()); + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = measured_state->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + } + } + } + else + { + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + measured_state_values_[i] = state_interfaces_[i].get_value(); + } + } + + for (size_t i = 0; i < dof_; ++i) + { + double tmp_command = std::numeric_limits::quiet_NaN(); + + // Using feedforward + if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) + { + // calculate feed-forward + if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + else + { + tmp_command = 0.0; + } + + double error = reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -picomputeCommand( + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + } + else + { + // use calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + } + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].error_dot = + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + // Command can store the old calculated values. This should be obvious because at least one + // another value is NaN. + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + } + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace pid_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pid_controller::PidController, controller_interface::ChainableControllerInterface) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml new file mode 100644 index 0000000000..f645738862 --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,87 @@ +pid_controller: + dof_names: { + type: string_array, + default_value: [], + description: "Specifies dof_names or axes used by the controller. If 'reference_and_state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + reference_and_state_dof_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interface: { + type: string, + default_value: "", + description: "Name of the interface used by the controller for writing commands to the hardware.", + read_only: true, + validation: { + not_empty<>: null, + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + } + } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead from state interfaces." + } + gains: + __map_dof_names: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + antiwindup: { + type: bool, + default_value: false, + description: "Antiwindup functionality." + } + i_clamp_max: { + type: double, + default_value: 0.0, + description: "Upper integral clamp. Only used if antiwindup is activated." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp. Only used if antiwindup is activated." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Gain for the feed-forward part." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (i.e., are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..7555cfc156 --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml new file mode 100644 index 0000000000..673abfe08e --- /dev/null +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: + - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp new file mode 100644 index 0000000000..3a75f6e170 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,43 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#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(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); + + rclcpp::shutdown(); +} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..11f703a1a4 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,531 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + 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_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, 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_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are not wrapped +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are wrapped +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + 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); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +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/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp new file mode 100644 index 0000000000..ab32f5cb48 --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,285 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef TEST_PID_CONTROLLER_HPP_ +#define TEST_PID_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 "pid_controller/pid_controller.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; +using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; + +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 TestablePidController : public pid_controller::PidController +{ + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(PidControllerTest, activate_success); + FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); + FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); + FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = pid_controller::PidController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return pid_controller::PidController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class PidControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + reference_and_state_dof_names_ = {"joint1state"}; + + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); + + for (size_t i = 0; i < dof_names_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + dof_names_[i], command_interface_, &dof_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); + size_t index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + state_ifs.emplace_back(state_itfs_.back()); + ++index; + } + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_pid_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands( + const std::vector & values = {0.45}, const std::vector & values_dot = {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(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.dof_names = dof_names_; + msg.values = values; + msg.values_dot = values_dot; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool feedforward, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = feedforward; + + bool wait_for_service_ret = + feedforward_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Service is not available!"); + } + auto result = feedforward_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; + std::vector reference_and_state_dof_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr feedforward_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..e0d3051226 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,103 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +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 d34a5b1375..3b77b7cc69 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -20,6 +20,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library From 71b06d01c1374914b93f202c80182210b385fb5f Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Mon, 4 Dec 2023 13:21:13 -0700 Subject: [PATCH 19/41] Fix floating point comparison in JTC (#879) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix floating point comparison in JTC * Fix format --------- Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9306b07354..59e2c408c5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1369,10 +1369,11 @@ bool JointTrajectoryController::validate_trajectory_msg( { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) { - if (trajectory.points.back().velocities.at(i) != 0.) + if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) { RCLCPP_ERROR( - get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + get_node()->get_logger(), + "Velocity of last trajectory point of joint %s is not zero: %.15f", trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); return false; } From 4b267648fa1ac10120cb597e7f6ee23593b6633d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Dec 2023 17:53:25 +0100 Subject: [PATCH 20/41] Bump ros-tooling/setup-ros from 0.7.0 to 0.7.1 (#881) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.7.0 to 0.7.1. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.7.0...0.7.1) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 76b2c22e53..f263cd2da3 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: humble steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 1aaf30c639..720f0416c9 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: iron steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 72aa9af66c..e55285b179 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 5789c2dee4..60cf153978 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 6b1af2b86c..f0ac7b447d 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v4 From 925a690fcb469cb705ec67185cd67c6f1481dd61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Dec 2023 22:52:01 +0100 Subject: [PATCH 21/41] Add pid_controller to CI jobs (#883) --- .github/workflows/ci-coverage-build-humble.yml | 1 + .github/workflows/ci-coverage-build-iron.yml | 1 + .github/workflows/ci-coverage-build.yml | 1 + .github/workflows/ci-ros-lint.yml | 2 ++ 4 files changed, 5 insertions(+) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index f263cd2da3..ece507aaa1 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 720f0416c9..8d77e65b6b 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index e55285b179..bf9ad847b4 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 60cf153978..f6b9c027c9 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -31,6 +31,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers @@ -69,6 +70,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers From e8a127f2b8bc6b5561f54d4afcb30a641f60ec3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Dec 2023 13:42:43 +0100 Subject: [PATCH 22/41] Fix rqt jtc bugs for continuous joints and other minor bugs (#890) --- .../joint_limits_urdf.py | 57 ++++++++++++------- .../joint_trajectory_controller.py | 10 +++- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7914a76b17..5655e12f7a 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -14,6 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +# Code inspired on the joint_state_publisher package by David Lu!!! +# https://github.com/ros/robot_model/blob/indigo-devel/ +# joint_state_publisher/joint_state_publisher/joint_state_publisher + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -33,21 +37,24 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): - global description - use_small = use_smallest_joint_limits - use_mimic = True - - # Code inspired on the joint_state_publisher package by David Lu!!! - # https://github.com/ros/robot_model/blob/indigo-devel/ - # joint_state_publisher/joint_state_publisher/joint_state_publisher - +def subscribe_to_robot_description(node, key="robot_description"): qos_profile = rclpy.qos.QoSProfile(depth=1) qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE - node.create_subscription(String, "/robot_description", callback, qos_profile) - rclpy.spin_once(node) + node.create_subscription(String, key, callback, qos_profile) + + +def get_joint_limits(node, use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + count = 0 + while description == "" and count < 10: + print("Waiting for the robot_description!") + count += 1 + rclpy.spin_once(node, timeout_sec=1.0) free_joints = {} dependent_joints = {} @@ -66,21 +73,27 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr name = child.getAttribute("name") try: limit = child.getElementsByTagName("limit")[0] - except IndexError: - continue - if jtype == "continuous": - minval = -pi - maxval = pi - else: try: minval = float(limit.getAttribute("lower")) maxval = float(limit.getAttribute("upper")) except ValueError: - continue - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - continue + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: + raise Exception( + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + except IndexError: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: tag = safety_tags[0] diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 99f43e125e..162977cdfe 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -29,7 +29,7 @@ from .utils import ControllerLister, ControllerManagerLister from .double_editor import DoubleEditor -from .joint_limits_urdf import get_joint_limits +from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description from .update_combo import update_combo # TODO: @@ -170,6 +170,9 @@ def __init__(self, context): self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() + # subscriptions + subscribe_to_robot_description(self._node) + # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) @@ -460,8 +463,9 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.claimed_interfaces: - name = interface.split("/")[0] - joint_names.append(name) + name = interface.split("/")[-2] + if name not in joint_names: + joint_names.append(name) return joint_names From 960e073185b15f330d5d9e1b7af2682cc7c2e8b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 8 Dec 2023 19:47:20 +0100 Subject: [PATCH 23/41] Use more mergify features (#888) --- .github/mergify.yml | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 3aaaab2001..692c346ab4 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,7 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion conditions: - base=master @@ -21,7 +20,29 @@ pull_request_rules: - name: Ask to resolve conflict conditions: - conflict - - author!=mergify + - author!=mergify[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: Ask to resolve conflict for backports + conditions: + - conflict + - author=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + + - name: development targets master branch + conditions: + - base!=master + - author!=bmagyar + - author!=dstogl + - author!=christophfroehlich + - author!=mergify[bot] + actions: + comment: + message: | + @{{author}}, all pull requests must be targeted towards the `master` development branch. + Once merged into `master`, it is possible to backport to @{{base}}, but it must be in `master` + to have these changes reflected into new distributions. From 999eae5491a79f9734f8d5e4e18ffea3e7a04e2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 9 Dec 2023 21:09:32 +0100 Subject: [PATCH 24/41] Update good-first-issue.md (#895) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 4a2664918a..4de9ad8d30 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -55,6 +55,6 @@ Don’t hesitate to ask questions or to get help if you feel like you are gettin Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) -* [ROS Answers](https://answers.ros.org/questions/) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) **Good luck with your first issue!** From cb56213c7836caa208c6049048088d33a72b6aff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 17:52:20 +0100 Subject: [PATCH 25/41] Add configs for humble and iron (#901) --- .github/dependabot.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc654..aafd67c236 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + 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" From 52d3d3f2f64c357361c8f99f018bbaa49584e27f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 18:05:29 +0100 Subject: [PATCH 26/41] Bump actions/setup-python from 4.7.1 to 5.0.0 (#906) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.7.1 to 5.0.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.7.1...v5.0.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 5d801016f9..9f090b48ca 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.1 + - uses: actions/setup-python@v5.0.0 with: python-version: '3.10' - name: Install system hooks From 0a96cbb513282880d2d64adfb8c3c8c0ffc84b5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 19:08:10 +0100 Subject: [PATCH 27/41] Update list of reviewers (#884) --- .github/reviewer-lottery.yml | 29 +++++++++++------------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 84b156f5a1..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,28 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 - christophfroehlich - DasRoteSkelett - - sgmurray - - harderthan - - jaron-l - - malapatiravi + - duringhof - erickisos - - sachinkum0009 - - qiayuanliao - - homalozoa - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas + - mcbed + - moriarty + - olivier-stasse - peterdavidfagan - - duringhof + - progtologist + - saikishor - VanshGehlot - - bijoua29 - - LukasMacha97 - - mcbed + - VX792 From e8e091da251778985ec1c36b24874f8ba5886d93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Dec 2023 08:12:43 +0100 Subject: [PATCH 28/41] mergify: Ignore dependabot[bot] (#916) --- .github/mergify.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/mergify.yml b/.github/mergify.yml index 692c346ab4..0a6e425a30 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -21,6 +21,7 @@ pull_request_rules: conditions: - conflict - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? @@ -40,6 +41,7 @@ pull_request_rules: - author!=dstogl - author!=christophfroehlich - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: | From 1d0d7531ef8d24ea52e761a872b8bf277652dd33 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 12 Dec 2023 09:51:22 +0100 Subject: [PATCH 29/41] Cleanup package.xml und clarify tests of JTC. (#889) * Cleanup package.xml und clarify tests of JTC. * Update joint_trajectory_controller/package.xml Co-authored-by: Bence Magyar --------- Co-authored-by: Bence Magyar --- joint_trajectory_controller/package.xml | 15 +++++---------- .../test/test_trajectory_controller_utils.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b41a7cad0c..70d9bd51d2 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -4,29 +4,24 @@ 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar - Jordan Palacios - Karsten Knese + Dr. Denis Štogl + Christoph Froehlich Apache License 2.0 ament_cmake - angles - pluginlib - realtime_tools - - angles - pluginlib - realtime_tools - + angles backward_ros controller_interface control_msgs control_toolbox generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle + realtime_tools rsl tl_expected trajectory_msgs diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4a65b4ad51..7b68d882ff 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -215,7 +215,7 @@ class TrajectoryControllerTest : public ::testing::Test } void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) + double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -223,13 +223,13 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", p_default); + const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_default); + prefix + ".angle_wraparound", angle_wraparound_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } From 364df81e9947671e6e51d45ce306b93afceebec8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:22 +0000 Subject: [PATCH 30/41] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 3 + bicycle_steering_controller/CHANGELOG.rst | 3 + 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 | 6 + pid_controller/CHANGELOG.rst | 164 ++++++++++++++++++ pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 5 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 5 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 22 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 pid_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a1cbe9596e..15e7408f00 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.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 615a457992..c1053639c0 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index bee3606ec8..89d3f6291d 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.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8ee42d701f..646903a5ac 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.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index f4496d3927..7cf9e2f114 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b87aaa2641..027158d18c 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.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b38f021587..e23dc6db47 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.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 798a0f3e67..4c492457f2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2c311626cf..55c324f22c 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.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f2295beca3..d46f807ab5 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.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 7e4c25a474..f2406abef0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup package.xml und clarify tests of JTC. (`#889 `_) +* Fix floating point comparison in JTC (`#879 `_) +* Contributors: Abishalini Sivaraman, Dr. Denis + 4.1.0 (2023-12-01) ------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst new file mode 100644 index 0000000000..af41de496e --- /dev/null +++ b/pid_controller/CHANGELOG.rst @@ -0,0 +1,164 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + +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/pid_controller/package.xml b/pid_controller/package.xml index 7d52121582..c955c4747a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 0.0.0 + 4.1.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 6f0870c9ae..ef93a8b3d9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index be35427e46..b530b7c721 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.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 46f994c792..03064c6f95 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a7a2f7a1fd..b6dcdc8d3a 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.1.0 (2023-12-01) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ee80d4b30d..5e0c8f82da 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) +* Contributors: Sai Kishor Kothakota + 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index bae8e6e557..44f3fa515f 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.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0203298259..98957eb42f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4abf0788fb..cb3a5af24d 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.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 91575e6075..8b84ddf784 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ From 2bcff2f87505bbc32fdfaa8a2938cecddcadc3f8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:51 +0000 Subject: [PATCH 31/41] 4.2.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 15e7408f00..975ecbf5a6 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 476c4d3050..20bfca8003 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.1.0 + 4.2.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 c1053639c0..86ba49d92a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index da5b652bd0..478b10bb55 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.1.0 + 4.2.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 89d3f6291d..d7259fbc75 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c627577ee8..2573605890 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.1.0 + 4.2.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 646903a5ac..eb40ec1db1 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 03dffe764d..5cacf41dad 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.1.0 + 4.2.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 7cf9e2f114..6cd3b74a95 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e55e095c57..e42456c828 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.1.0 + 4.2.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 027158d18c..a808f82c70 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 48db0e49b6..154e597c5f 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.1.0 + 4.2.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 e23dc6db47..45e5a8081c 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c500c1f714..324ec1c5be 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4c492457f2..ff2046a41b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 34c7ba663e..ce981141a5 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.1.0 + 4.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 55c324f22c..e8be87d1f3 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 3ab9c5fff6..48a26ead3f 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.1.0 + 4.2.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 d46f807ab5..4e92131965 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 37ba7b4912..300f8b2238 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.1.0 + 4.2.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 f2406abef0..1b3559af58 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.2.0 (2023-12-12) +------------------ * Cleanup package.xml und clarify tests of JTC. (`#889 `_) * Fix floating point comparison in JTC (`#879 `_) * Contributors: Abishalini Sivaraman, Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 70d9bd51d2..1d0929b8b5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.1.0 + 4.2.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index af41de496e..5924bc6d94 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/pid_controller/package.xml b/pid_controller/package.xml index c955c4747a..7caa3f7856 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.1.0 + 4.2.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 ef93a8b3d9..2e1e38fa43 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 1aec89e59d..8ed752c536 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.1.0 + 4.2.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 b530b7c721..c92b4867b5 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 222d87e3bb..08e8fd9506 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.1.0 + 4.2.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 03064c6f95..5839d43b68 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3b77b7cc69..793aea9140 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.1.0 + 4.2.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 b6dcdc8d3a..d7c37b2fee 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 16dd709c25..84ebaea6fa 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.1.0 + 4.2.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 baa01b97c2..b9aa443081 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.1.0", + version="4.2.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 5e0c8f82da..67d9eb85df 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.2.0 (2023-12-12) +------------------ * Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) * Contributors: Sai Kishor Kothakota diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 24ed2c9d63..4e7c622798 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.1.0 + 4.2.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 2d855d9255..1e49983693 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.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 44f3fa515f..fa8cd4ee2f 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5de0ddc872..85398f1d90 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.1.0 + 4.2.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 98957eb42f..d7424ca31a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 5f382bf8d2..4f131da79c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.1.0 + 4.2.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 cb3a5af24d..3d04f2ba8d 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.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f565ef0c02..f6eb3fff49 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.1.0 + 4.2.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 8b84ddf784..98d6523639 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d7376e8e6b..3d901585db 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 13a43c32b5b4d0040c96d4e192db4f1ed6d4f046 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Wed, 13 Dec 2023 07:16:11 -0500 Subject: [PATCH 32/41] Changing default int values to double in steering controller's yaml file (#927) --- .../src/steering_controllers_library.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 8acdfb1448..a9f7fa75fb 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -1,7 +1,7 @@ steering_controllers_library: reference_timeout: { type: double, - default_value: 1, + default_value: 1.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } @@ -93,14 +93,14 @@ steering_controllers_library: twist_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of twist covariance matrix.", read_only: false, } pose_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of pose covariance matrix.", read_only: false, } From 691175034c8de94dc70318fa3edb9f3e889434d5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 21 Dec 2023 11:23:49 +0000 Subject: [PATCH 33/41] Bump actions/upload-artifact from 3.1.3 to 4.0.0 (#934) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index ece507aaa1..a1d159541d 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 8d77e65b6b..08e6eafd82 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index bf9ad847b4..f785652989 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f0ac7b447d..fcc1a297fd 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 4c6d7236245c527f275b1e03a7e5242b658dc782 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 21 Dec 2023 12:29:46 +0100 Subject: [PATCH 34/41] [JTC] Add console output for tolerance checks (#932) --- .../joint_trajectory_controller/tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b4cb029dd9..b5e660be54 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -150,7 +150,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 59e2c408c5..c6a5169855 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -247,20 +247,21 @@ controller_interface::return_type JointTrajectoryController::update( // Always check the state tolerance on the first sample in case the first sample // is the last point + // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.state_tolerance[index], + true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && + !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; @@ -269,6 +270,10 @@ controller_interface::return_type JointTrajectoryController::update( if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } } From 3c3f61437b89d53f01918d6e49f4428112609824 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Dec 2023 08:53:42 +0100 Subject: [PATCH 35/41] Do not run reviewer lottery for dependabot and mergify (#946) --- .github/workflows/reviewer_lottery.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 2edbc9b59e..ed28964e01 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -6,6 +6,7 @@ on: jobs: test: runs-on: ubuntu-latest + if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' steps: - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 From dcd15ee6bac003aecd668943f3d4c93c1e0ee171 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 29 Dec 2023 12:40:21 +0100 Subject: [PATCH 36/41] Add rqt_JTC to docs (#950) --- joint_trajectory_controller/doc/userdoc.rst | 1 + .../doc/rqt_joint_trajectory_controller.png | Bin 0 -> 29746 bytes rqt_joint_trajectory_controller/doc/userdoc.rst | 12 ++++++++++++ 3 files changed, 13 insertions(+) create mode 100644 rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png create mode 100644 rqt_joint_trajectory_controller/doc/userdoc.rst diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6a34185437..6fe2146802 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -193,6 +193,7 @@ Further information Trajectory Representation joint_trajectory_controller Parameters + rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst> .. rubric:: Footnote diff --git a/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png new file mode 100644 index 0000000000000000000000000000000000000000..cb86f4620bcc091f87db272c7868d16966b091b6 GIT binary patch literal 29746 zcmbTdbyU^s*Ds2JD5!{(AYl$F$c#zx0lPtV-O(86}>Mm-M- z$`cf!_itq#W7nr09A)KBf9-4yGx4Fnh<@-SiR_xUBp*cy>NPDn4yqzbwFgoP{PJ(! zE~fied*=M2O8!76Eb}H6`wa(HX6Vi9Ld$nnqOCnVbQvEq2$5?^e>~dbI{FknIb*ZQ zji|Fxd2{=z_XEEFi$BKS$~B&-UgQ0B!}2=Qeg6;0k0iTBf1-XuP=Wpv-{D$|Sb^-D zZ`IWnS3kSe6~^X$+q`k9(e5zk?xZ!9d_Y5xRIa$QZp zis3nKf8txiWrJ&%mvM~6MeUuvt)?s~l=k@&|6aoRm892*-~`H=LgYlYE( z{Cau;V+U1lYOV4;zITYT(_>rlIvZcp6fFN1>EvHN$W>7>__uft7`|X+#I~^5`b|qq z+rVl*HGbp81Y`a0-h+y&>S~9BOy$zOxbHaZI%F`CEYGDaCX6_rU+|Ee&wY2Z>KR?!X|Q_BMxNwi&z^5H4EFTA0c;Wr(GI{-zN_9 zwy}R<|3kDF|8{M3n2O$z#?i>eI!cB71C#N(NAw7{Ul_(joF?)$d|bC(OYlxz^ap3A zz01kBUZtA<7+x0}DkSfSe%v}(%|>x20=W?+gl=5LkL1a)Yda4o(x{X%B%E{yimh4v z%{hu`JMvVr{KOto{~Hgd^+dOS@_ARD(E*jufE*+JGIe+T=G?a8i-w2E$ucf7^)`Y( z#&#BVF=q?*{8|nj__(Nkirs#E>Z~^rr#ijyGMM^dfbl5jv1mlC1QyS?Ve$Bz!`c1` zHm~)qO*)t9SoemiJQx#$;!X^NmR=gU9=+E1B>c!ji-=_YMzU;YuGxnxAq)CLi(12l zT-xJmG&zFugok1F($5CT*N-UIF;i2s#I>Rv^k?YgeAdy90$h@W$$6C9c)E`?H+}3S zE06v((eM_nW_Q@`71u`@CkT|>-D&PAe@XLBb;EhY?cY1e;_~UqzSigHxIW;@TD%O4 zXj513iN83T*jX8JoTz-xUUngyjA*O=GF4yyORKH^(ogV?{jP6rTn5!hQrC87w^C6& z4d+pT3qr!R+>zzVF^T4~<>XbVPwsq(1@@s!)M{42A%3NYM`n&qq5*kT^;V^H=1y$U zqexn>U9q)6y$A6!cnG1QSXuOo8iqYF7haMX5pSw{%62=|Z@O9z&d)T$m*?Dq#5gy{ z4=De$Ph|1=56JxL=~Exf-}oy*xV}4LNrloYEdtUCqSoEi=>!N()Ds%i!MAa>| z*1$B2uFu1FGCX6*{U--ZqB)FtD7t5u2F ziAw8h7^4^Be%d^t5V)7W6K~b2lgL`je+ayao9;sFCfqX+s&-k!Wvy#mN=ZCb z4%lq**cI-pdQ_t}pihByG-)5v6W14kK~UC}lPP>$JpX9NmXj~b_(SdPKZXnXjt9P+coS&s#w0D!_>6#}rV$0Zv_FS!Mo}UGqb2_=PYUPX1Wh(bl z8X?B%4Op^|t{U!zJ^oy!H9SulNn>xyYJ`0f-u7DLPdc*~@68-8xkI#B5{IQtEWq`-MEFTi=l}x3I zG1IpRtb`BeUPoAIBs@QxabmjU8m(i0YK}gUDKyn#*_5r%CQ*BW72$0VZAX96XCy+H zkb8ojMQ6hzH+A=TmGMJW#Ap9<(oz-7KW4MG+E-;C8Bg#w1%|AOPY{28F9&qIQ0;4F zEFim}7;EBOb4t6k#z4g<42bQC)k><+eENzxnwsLqrZ9S|r?%{cMK!K`Bx-nrt?#j0 zy^iVQAcZhXW25^v@|nVlLq9TbpQj&@zR2tkD*xuVG0}QdEru}}>0j-E+a9zb|A$BW zNZ2p^Sy_T$P239?ySdEUbi=}UZI@>(#VpNXHilL2I+JepPF7F<6dSL%2w!}?hP9yf zfjq3z4VOx`BKQ*J<9F1nCpu@H*VFTpK%nKg1&g{QDC0qR;C(tl;s%DSTW47W? z8I(m;FKtn{4c0Oj7C*%}7|i_O{e;K-qkmkmCpyN-K;ew1clLCc&w#7r9KW^j#c%H| znPSCVd6VC~)2Im>CKZ1#*q-0=`I9_FzN;}Gb*Ofiz?gN)c13ajz^panaIFo;WsO0D zfI8!7>+knMb}{3UG`YYl>rH*^#&>p_9Q+HPOmO7ARHa6gj3SG$G$Tpo6MMuvUu?6o7;Q5MZ~O4foFJv-Imcg{ zKV|3%#SFb3Kg?0ks>JR%?XhY(W|ndN{pYjyvYcY*{q3gS?5Oso!H8TU)|VNteERA8 zgR0;5j2&MLGgD{mWXu`OlD@Y75_`(GA+q*5p=cFhc3xpcY*OdsgGWmxSvhAsKSKKZ zPup!f>7+s4*6~yT0Md@5u-|{uq?4Fsn2i3i(=Lf(LueF@njk=> zq;{Eq_v7L!ZP`^sU~;C|S=qG;79%ko1Kg#(>T#FK?cR(6M-iMcH;WmKP*W$TQlEXzMt z8=pQ>omQeJI2s`RvwQsQdV0N)^^}_>W-CFCOXe_p;FxZ+q9kwXg8eh&&9%C>Xww_| zMKgBUy&c9Pgc^62Z2TR+I>eiNa#MeGYdl9+C zA(;Cjp}msyO}NT^j0l&tjKoJ<<+L3 zv53I4_Qz1tS5CqEsfi;?7jh}OsiFF^zgmKSzctU$mXS0n)nveRe))2?syAW5)E={u zTV*Iaj8xF0POOw<+9+8r>1W*heoLs@Q#vEpmIR4FhlACM%m&*-!8)a4pP1j1OfE)} zy`P%oqjh?*%vQyp$Kh8sn^CJBp&8-zyPVEzi7r&34i_T6zP3ZW?khVlH9=`()p+`t zIGoDp!>cC(U2bEkJ~5mu1zV{R=cx@g0hx=QosU{1Cg=R7($e_mdMx`{XggR+O|GQ7 zqiE1233z!GWb3Ej7VgAxPxcH9U(y_l^xNGh3Lu-VeT~(VmE6d;lpFK&6>k0oj&!JC zdYtNPTV5F5z|TxZF+sOq%>nE=GajE*CY-sleFw)8gq*!XG3~EnxVP+5FI*1AJtU$# zm!srD9%c+W6MNC17qE)8$vD*O%BHqfPKTXrR(94>bY)&>bR0V=#Z%mQK4g2pXN$D` zX4k_r#>c_jZ6-9vgGaNoLyP(>;&NOAO$lS?V(IJ;!^Ou~;%(#VBGSqO*itQ)4w7t6 zY9A@Qryg^kn>@I2=kbFNV@IR?hk?|#;Zyw~9hD;@Uqw+ld$AD;gHW0J?C(_AA5xc7 z-`g?b2|~rFeh@h)PIKhx@k}YtRKFwQ(zU;ukofZQUQ8=_SXzbT1dTt9hes&pk%3NS zjgQ{sEryh8Qnj?4UO1H zMu9VZfkU{Za{qU0%M6SCrp79p_Pi+Nz3COEMU|OuqHe)6Q&)VYeR~^f?%;c_VVPbx zB$cQsGo-TRG%GuU5`qz`w=xYk(%%G2au%v;HPv5MVn3(YDJT=YJVC#`f;XxRGwf0wIY}*OWcmJ#FieBM7 zmo`OC+Yt~W{U*Rza99tIVl;0)Kf0QJGwtUn%R7)sf^1Zoe0qy&#AXq zTmCm=@#iP{WgA=T6Xy3*$u`zWo=H%I8sDT|nL5|ueU)C{YJfJlPNyNnmgP3PC}m8c zgoxZb!q-6T*`cu|IQoi)p~}dkkI@q8AV#sLWK)G=EPobp>2oLN#mG`uIrY8THhZCf z8mMC16dgm}miGQ|Q^PEV+34X)KQm{P%_cShqW({dmWEZ5cjUWa_Zv~gr*c2Ct~CD^ zXt^G=HC?@E^ODvtRSs;q9*!^a{0sJ-wfHpJDw|1I50|FF*ZRPc&SI+a{!aksry%Dl@fr`kgFbSm`<>lk8; zNGqyx;&8oMZd3GZ&izLR0=5$@;@CV?2dDI<*iT8U_slCoH3zr(!n=9t@bR9gNbQW= zGZ>}dJRH=_6Qpa9uAEcIW+^qB;Z018Vyv*zN9*D}@th~CbGITz_m}8+HahZ&qO?>l z!+5*7i!9r<%M{ZNmn%+enJYU{{L+h9glFl>z48R{@~|kU+*d5g&S2q(l5c)){G3$g zm~Up>?Va0-kqgZ;vcwm)MTQ9dpR3#w_%rmGUqwd>SIe-=yHg#~hu_(Jl|(A_IdiIC z5(Y_rEC|z4u`s`Efde;g8p_DqNHM;Wj|TSk67ob!_uGFpL;oY5*`lUdD{tuP>sv2&;);@osST~!a%3sw2dU@N zNm(L4OzC3K8~c=u%x7Uiw>s@;d;8bWP?+<<>f&Skm~YK#+y?BfRPlRO^7eR0`9I3XL>1{&HB*#w=}l+8_JP@RSuFjTKCE&zYCE~gcQh;&<*wco$L*%d+a%33WxKE6AaeJHPjR;@~Q;o50xq>za2}LAnlE1Z;I!3IsW5?$!52Tb8&i5?1X&|nLx1np7D^x|L>b&G8FAOi@kjl ze*y+QX1a^nTPPe%+5CDA_`Rw%4PW=Z)*Rc4uxzlaq%B@Y4k5v{bF(Lg7118@oQTIY zs-S@0`Eb4CWDg-yGc_02Ag$ET@kxdZx7VC1%egwmMn2@{)RKTOUfg4}zpKe9MZVad zE_n>&WGNlu8mh)TF84@(Fd6a1ao9a#F@3~pF>`ZQgT=T%@mhlW=}Wb0donJr*w;F( znoNY(9x>?NR#H-8GM|cS4SZ~KXgU*E?0jH6ny=9Z0Vq7*8W_!Hr5&$lzdHPNG*?B3 zA=>fs1d*wbPxAKdTjairMA9ln+H8?EGMkL{SJ|0dzj4FqbS3?3U|?wwr}I7~a%J$S z_vg>MvsagqQBjZ5M57yLYF&(`Dqe5=F1_)Dh9>YUHpRn-5Ba=q-HYOJt75iNtZ{q| zyQ;H3URYu}5lUBg>{(^E_7E3WPi-t$rJVlJUDaxP8jTBmJ-yd%r?!oajTurYx2CJ? z=0|g5DcH2NwU;N$>EN};8{=5iosheo?kCx;jqWcGc)e1{!%>_)YCRcI z(7;vBYkU+apx+(!Hbo#X1D1zPF8YLOm#44l&v!9Y%1pH`&re3}Hl`}W4C0+Wd!b=6=yyLlJy>hY zlFQDSuRq)5{9SGloS$z|#|F660eLuHVuX!Pr@k;B%qx&TrcAp{g4Dr9MRXLb)!s+O}A<3pTRaej`~Tf#;<4U~McI zM*HqK(IQ-+%)8W<_~kd(Y$^#)IQO0HAsgn7FG^<5o?hGn$Lp=A^>vE*{JF&>Ez2{g zBwIQ*PT30i`okGgH__1M=jIw=s(r9L3|B8%Z{G3a~%EE%T za4KK=&`vjF-=C$W&&9=z7PSX>XGdG*?6b-I{*SnwnY6UDpvTY}L@dI`cm4J2*EuTX zLHa#EUhB3$*mve8At7Nh`(5(u(P)lR6X#ZqS9&_dNS56Emd6z*BvVO{#o*67#2mIL z*6we)_PQB!l!{xOGC%C8a)TPQk;@SN9T?DcF9G5%VX#2|d{8|i#G0?xIiEC{P@LCRW&Y$=O8LTd0(!PEjLda~a z$a{8rI=8U!9yT-ll|nCTZFzb5!4-61Uc7jLQ=D-!t(K%;91ZFIO)Q25;>}MvmmI2L zP7ZZ!Z0v3~K0ZD&bAD_MFGCHhuwFzjH6AH4xma0Qf$j{21ov=`5~IY#=Jc}I52l8$ zE`KQPSdSjHM>85Mk7O&<=^5ziTAm%6#T33EC)cKb)W2fuH0pAGoFHL2r7~+|WaJaE zxXHHhGLnXXfMBY|DGV?nalr6nZ1apElQOojN+5K`-cM9JA5g`|pJ+T(&i~9>e-ZMb zxF6+N3hiQS%b?MYr2{$3(`XTet%9>=J=NF)!%{*fLfi)C?A^9Sm78etG*(;`67dUA zG*Im8pNx!*;8IXflJ?iLftdgJ=_M{Jf)QUrs0S!tp6K}np%_07(H1#K?m zfPHvsRkn9LJUn7vFF7wo8z`0<;{vcP6?sQaClxrmBy<0cFe*Fr# zVG?S$<&2>%q4rybR>fLJ9pLNUV-{*_Zs5ljENqb7>AzYi7E$$iiKUUnP>XShn^W!Q8OKh^vQ=jGS1z1q z4|pYyyZaS*TwS`JEJnuidc*_YP7{yogrXVb>nm7@E}1Hf8zAs}XJ-dhe13lZovp2H zQdgx7;=a4P`|Y!?Y=G#w0bwHBfG<{(Up~?YW&e5nR0lq#Y!P_&CBB)g7$}W%+ z0#acu`Y9Xy|>hpqga^ytIG=o zfJP5AfNGs90gSja-zFp`ma5hC8HqQ29+zS3>!$aKIhubko2)zgcyk}94O+hypB9hj znsAnJgKz=_5eeA?NkPf&7rMu=1GBhK%F_+ppx=-QS&>fCHi)qr=fnh!u<<^0eP-@A zW-??p=6Y30ZzcJv^}sAB+nEN(i?T25iiG9Sv!h&J2h=@di;Czft$A_FT8c0TkcuxYf4j^Dm>t{GS zIl&?!3F^>I28c!#KLTnplqUArd4KsW2qBA_om&DaSb+jfKpxm^mmeIgj?4q?*9JUb zvY4sqKk#%zp2;=QA534%%q9o373e@o_J&FDbgT@e`^N%_dcSgVdA4=u-n~>PR-*A- z(f$3Bqj_p3dlB(ZJS5`R2BKCl+?S>Y)C}>I?*$433gBw!v;6m6fIS56?dhw{F7(_L zv8(%bQJ1qCj%6+>)+g28fGOHARU!`CF928rHBL6-v1|>&yq8g14V7~RN6sRXP-FS) z>h|0=nV39wWTDp8?+lv3Ere$=fBpHEV&pJ?{tGFSt>=q+s9{Tr;T)|*5&9S znyPwhFsX#0A?18qFx{3UN)(&bGj(-!(HNH8ns->B>L0M%E`K|aSR8lpDi=t3D}_U= zTJZ+>PL2mv+n@Oc7o94rxnPsn<9WgzR%t8yQEb&>s=zhb)UtXsQtB!3^~J~3QB!B1 zj%C+?4?5bc>DSSv3&&f69v(ikS-WZ0?8&-ZEpsm${@}HG8|;x_`qMJ ztHbxbQkvb%Zu`$$7K@qg%ZszGK|%VUgivaZW?YcM1T+#-VS>Ogj5AmnOg)D*OvMU< z^*wzbO%;CA1Rer*XUjnx$LX-t{1qDoege&dJa*^Wc&Jh;h>3a+Ub>oXJ|l0SM{x5b zO_;1<_wcanau%o)oE-|!`Sv@QE@;GL&_g-OrCCh!bpIaBE77ZD00>0N2poPbp!j!* z*uII!ar|9dQ;E>}&l0j2-25y-qN&!q6Nd}?1P5apmlEc(P_%``O#O4Z3Mhvd6(LaO zdst_YcTY4lG?sefOlo5*Y*(bGrlxLVV$yYeL%#4Sk+>eh*34qIj#w!xLB%! z|BRR^O9jprah0g=tQT1%>c1r?TG?JoQ?XWd>AgiBGZ&@sbBmDwGWq^Ld3*nJWkLn$ z4@gLQ|2FwvQ>=AnzJYe18u7*Z0VetiFZ`+JS?YRXz0&Xffb7|Y?}J7xlFpmil;mM6 zNy-vVF0!33Z+^IX>RljFNudbXrMJ)?0&-ls+#IehJT@RcIzc21LBaI&@^ac~!LM}M zBS!)5Kb)r)!)~+GpDKd?gVmxJk>GLvjj1U$6xrtt454XaF@s6(&`^NpHm^?-$TY<(XPe-i!S(yYt_rCPrg<#6XhLAg)w_k7-;8zJI?kpR%l>@t@h+GG6LLp2IyjVzvm=A!?Z2^{4ng4XBm@vL`!zSek69}uV`dHq^^F8PFj5`o16iY3 zEl3~>bYU23wW{xcTPkaYgFl6kJ}Xir;NrG;T%FBwb#VfU^PX?t2nh*kZf@S5c9_Lw z-%1sUe8g%_RBX_<1V{Vm?yWVBYxY|+J#c{0FfbH21ON@f2Veb-6pqOSsEh)GJ`&gi zi{jX7hb{f({-pJp+8BWRz0Ozpify&$yItLJoRKg`6YH1nt+=uvNaq?P^X`noGwg1s zOpFG-&C}DwwdxW8Th|ne_3u3w40^%F_6Q_UnnXgc1nKH za&a_^p*D-8cWVb3vQ#YQQy)OPuRs)V#05dF$W&S{a@CWYDV3WOgS6dW8A1a~7hZup zc;2h?;+}(XAh8zp7cAo9;#)OxE8q_x1YR{&uK5t14CjC zfr!k%TjuEGq;!=JHQ2Zmrtg>#cYCytRB?V0dVX?Ao%$rDVnGMYl)O&H?yH? zfHM9T7Z;aad;@9n)wxcbo&ZzUH87ap+Uf)A4_z%!+3c)&YP$Y5s4tLJos2xNV2MyI zkvDHpHYUpvF&1?%EeQoLcU`w?k1Xx%^!nntD=I1qpsct?-WFlVZ_sSFA@9~MTwrGd zVL=bBmp~w4xJ4z@yd$+%JR0Q!>qX(B;^Jsd2U_qyq1x@%MZ6@T8T5e5lttFH?-fBy zu8!sREp&u|Kq2GgjDdQL(x0!vi$|;4PU>;~RzTnutTaWX+}zR85vOtrtgDCk_y)BT zlMViOQ_^zmK=bm(eB0enG5HUL77FY;o4NW$jNuWNN+aY1ooUV>TA;m(LVY)&Ns z`od30Sb{P_%wUnPUONsJ0HiHI8vwqbv~;N5IfQN$Ou8+I_?;Avw6yeifz}N?|HZnl z?;#-#P}JJn+u!7m%`Yzc$gAX4);XdG0W4&+cQQ9fXUe+n^{`G=+4&>sbAi^o2x_J4 z3%2i{3j}C_rH}N7K``>1tut0fJjTL8$^igKy6qwvVBW`I21dWuxdnPa2izWzbJtl} z%lRTV=I5J0nj8av$<(OlfhXs{11^8}5L1O*><3cEflq-CnVbS?b3d9DbPotl2Cc@M zz#l(|iv9%cH_CVoIEwb^{)%WU+do_>GgATLwEj(F%rCjXaNrq<%ld010Kg^#$Lw{Z zQ{YnmdG&x#R`-yEW^$&^Z8C3-6?D3_z}z)mmKY=O!J#%nb*ZRn0`~s26AohTaAWe( z-Ota`4|aEr(biuXS6O>tMo{u1J;~_k=qOS0%Zn`!8IZ{ECY2d2WZMdpi@j(6_GFPB zhE8h$um~(d!cGMZH@QFe>X4+PAVKvR4*XB^b1_;h^nZi^<^R2%_rD;&^Fpc~3Z=A+ z3;?mdlkYR{j3-&LnS8LXdly=JF32ORh^)O3G{hm%&o!-xE^R%dlE<&jIl!dUu)B;7 z91ErkahpTjRGs#=w>3erj-KAWe_y{Nlni10YpYCI-Fv5>CH>bot0BQpnF)S~3r%4U^Y_H$3QvacS&8|+lIAUs5% z53ylfOlCBTDrO?_7|`%t;g~}IsYq3T->|%zni|1t?e}#ro*fnE?qFUQB=)sV51*H8 zk=~5Fje(>Bpti5Q0XG9m1Q(!C^XzN_EJiUs1_Tu39L19-9}N=RMo))-`C#Bfm2}F(2coM7DY`UIQ`1iU_yr=e!bq~G5|^ie?FrjpCrZhdU=fRt3? zXmc7ajZ;BC1zN+X?JEz^?h(F+42KDEoh*bQDOX<+)DnJZ7$U_vFc&b&s+YI{?!k+I z7w-&QaJ;Z~c({VA>pmu?LGu?Z{<3nY zvJ57p)N2SZV+X-Hy?g5hvZ(=ef7A;6V5AKKWXE_Us|Coex@Nb}NE`U#$7>o_onNq? zgPzrysd19cSLc>Ua9A6qsi~={fL7TjJ7f`pet665;}_(ly78(tj2&KnY-=H?mjNcbq&L zGI@2lI`Xe515W^ksH*8?&E46Sv@1J$;rl$?#ZzysDs#Y47i8M`BAo{)dwRqECsYb~ z-9T#ufUrY(1vEff_Q3v*H>4?~^y`e%dIBsk5TpsIa>*xpx32FUp z^cmH70$a8OB;>uIAoha?zr62Y?^3V*`g8+1L7-BWe;ZN3-Bnts(J!7*K7_slvK0kw zIBS!Wjh9<_YSmzvp+Q7b9}70+xd81=^R6OkIJfh(k6Y zEdTPsnTSHJIUP*Ed3UP6i?c)J#SCjWc?zk(NR`vr%jskwaQNWGroesLLP6D!b0kaDG{@pv75=nf6Nx>FIRuxFz^d8(2TI)a6j@t&(s9j3DL z>!4vIm!VWl4%;=#i0s*O0c(O|B;uC>uE}*YWm8dE`FDN28@L&4p+>x3?f#&MVX1P7 zG7UJLg(frD6ylcit@qTO7IBA-nZb5Jnr7v+TnfhrqGM;hxDQY15-tQFyPnCENI-;iEC!r_?fv5Kud}<*0rrn_xp@yH3DSP| zOHLeG`4SXFPD^_~PFxWDb+ggTT(gifHK{cKW=Ls}5`fmAcj928xXST#24` zR9i_Ur@@2roA~2qa?NmBxoF!J6>MM?TUlFovZ_HNi`jU%zsj zR#sb%IJ=tS;$r5t@=}@WNXh5`5rXVb^)Y?=^l4NB1(kUlDxcNywiaAX(2k=6p9U7S zX6uljPFY!xcTH7QbsL&*d+anc_xL+b77xKDF4k(g1w=?Hd*o+YT4;3iS9m81I73dW z&&#QOdLEOK1~+KFfh#bGd$(^>$~$cZDRKR<=4^u4SzIC7^3A<`epEd?EDQEqSb8wT zjeh)L3@5i_@@3;p1=9VovnzEskWlGSYlULOX88w2B!ar5I0(sGpne&T<(V<&x#)D!Pm*^3u{HaCOn>v@%PStRaO z-s!!%n7#UMz3=MkdVDr})e999L-g6^g}M~Z-26O-8k#2e+O(%fJ=9;tjzdK*KV|o~?t~Lpx)HyoP z7lS(!&-<7FWEy8@>-#^GJb3WN_dc;=nW@sJE?UFsvMCioR5x{q6QBS(5F9Y>w{PFx zSX$EK`IBk9qwcMNh7Yn~$Pk^w`W7CH{`KpZ^virl$Aiu3m_L6s!6zav;>9@>L3Za>nJFseOZprZVJpe{`lNcm$~od=@SI5`46!0uOI`x2L%KeSWNPr3?UBo6N6n1vzxH{?cST}wT zAb@n))5VDinG7{;PT}pvx*ZQ^+>X?MBO*&7Byz7!R(+zt9?WO&U`zuLjhOGB*MQJ^}TrRPW9Wg%2qk;P?2o z>J|=8{xbRmy6wb%lO7AbVP{v+4V27PW`-yhfVf=wUHS^(wUFJ+>dfEsGm;JLFH0E@ zXCSfNjJBH1tQ4l=y4`>lK*}2reu{F5VFQ3Eil&(hG>)D@c>yCr(ITP@*T!n~xdW8O z=UiMwVI8`)`B@oArn){^9*j&G*!@Cq)WI}@I~7Q?4=kr}P)^8p1Tre%na|0T;4%(^ zZbo+FfX2Y~U3x`ED%5RUtY+u$0wl+l_4T+~4te_T7w*!G{vJDZ?ciG**s zTmc=a=6Axm@<_~r8!4tXHMy5J62TFJh+^xjrNI0F|Bn>UU3B%TGf61)p#OneJA!@V z7kdbHjuylR3!T^euNx4e?tjcRLZeqTHD`;pf;?KJ{Cf=iOp*Qcvlp)PkGT~u$?i2&!`GMPXM zI%?`1z88w5Zu?kB#@Fb5XE;l)i?GjStF{dea!+iw+Q9;X_7hq@ribLIwpId6EBuVC z5$3wc*#b=)DyYV^4Y*ooy;xk zkh=Z0%KFNGya8Y$OW=%x53f9n_RRYMp1%(u8`3(xo*n}3qn1DSg`g7Bo&yI>wZiff za0V+Nio19DZ%cgwrw_^D04mk~F#UUbVLH-Ha1V0<+oqHA=F{dI!bm%TUvBfiS~-s6 zAO6Q3R`CD6rW}4IL^#7V8@-8=-^pD8f z;U!Geg@uZ=>hCMvi9rQ3Fxm)kaHd#02rkZhanL`olYBtMS}yI+fBUI0AKdMJ@1u3O z!7A#Dsl#rE-nH0exrQffOeHLED@v3d+o&`{RaH_h>*mZ%Mz(_BbmFz&+m=j7XYv}{ zJM}=$^ZS?QeVR&hbM&+tK}LNfAB3-`V1?(?tvVk_KEBItkfW+*Ga3^g6dLMD_8P0J z#XrhntF^|1x3$z{K;V6%%M!sbvoRYr3*3*mgY1(o3J3`|mY5U6@1-7MH+0-FTN^cu zVHI3lxuZ;D$do9LxQl`Z>u2%m_+A|FT#%la?W56wv8c;oB2e@V8Hq*ecVM4=LLNeq z0fW}bJZ8~SPt!w!ugPPODEFu5gT2Hf9h8gN4GQ?P`Q=Kfsy3$w>{dK?(i7-OxIP@l zvPT6IzvH#1&f>6>G)m3J=KD^wVh3F;`Q$F|iHWHD#Em7EPnWPMBo(qh*O@4+ca+`y z@4EoQZ{Jdq4<#Hahkq=Sp&0)3=~1~^Q;7>}XB_A9XG}qClKakXg^}sfnZiW=59S8Q z^NpbKN*s1)DCylJgT7YRbGB7N=6;yWZ_SVE>#-E5TL$gD0vlTi#ob_Hka7wfrYC6g zxUx|=Ve@@qcxC|mrPsRKMjC}j+iZPEP33jX%pWhKrdFz`h^UGryqpzID}aDNu{XY@ z!t&`Um7JuHH=8Y70Fruu+-tIAO5s9UEGi)(_9IndZuGo7!_|>@iyLt^OIbJ>S+Z|s z@=4M-9q38f`3`HGmea+;9?$wb(EArP(exJMAB!C5DAB3t=!QqKJ`H@)X2N67FZ5%L z5Z*+|kdf)1uF)Ig7#Yb*@(U6Wl4vV3z+Afbxc@$}v&JIyD_xKYM`;&PQ4y=~I!ZKg zDCzJ}Aa&sU#QrdODPNsf?BWz9N+RL&s~!8E^g%;O7Z;s6`+F^h%@y`EtG#iH+)8|3 zq>T58&#N(uNN)w76k0NP@XMEfuban^6+Lj3cXPY<-`R0R!h6L zpI$!>l#pYxbJ5{(dN^{pk%686TeRqVcXupnJ;GgFcco4vLv>~g~Gaog;LN`(Dlv3mZwyB?Dg z@~cBzet+8x`q=CDiZwjco@ride{vn~BfpTIUq1<{!W?UTNNcP6kuDdli>YWtn~lKP zd%e_k-f`Nt)<7AtCM_dZReLV-m1(RTN70=fE3!td%wf8=G%?}vvWI;<6m6L@2BY*W zEG86=CZ?vV`hz+;Md%p#I~~_FwM?1NpXjXjicd^W7a7c~l9NPoGcec7843rhQgf7=#B*>f~B#>&j^Q1$iBM8lsPshsA+qYf%cNmpy> z0oIvsXN!^Hyj;|;9)=M6cZ!e8@D^s|e0Y!A)MQ=1UzBV?Bn{8We-hj=$&cf9#=R?S zv;NQ`I8e-N`dgy`%Ey)`a6pe-Y;8Ad^TZ#w6iu5~j;cTFkT~Lv2XRSqaHd z6g}L((tiE&VKll@Ln67l6MtdDTcNUD+RFE3liXX&;V+c1v5S__-BC1eU&DB_^#0l4 zgn7~VrgBd6at1j%hF%wuuRWxDc50XH?9z`!)f3k$~XISi42#=qIg_$~= zTw;#3!lJ(A(I)l!1t|l=hkvhsjj12aZ4I<`!P3*W0dgGuioQp6~M?*c9yU z>f+-3ys$!fB6pJ-rqg-*o17{aZV}#X#k@=RiTQcyCr@zkUs-jK4Q4uow!QNAmt%h} z+w)zqT)fvIxTURaaeYgp;_%#cf}cMCx#OeoN333#E-o(3IJJkXY6=>raA?k;G<%*~ z!uswms)hlb25sBf3`KT{Y$(&=>D$HNV7g)nkc#&WVQXA* zASnDkrD%6=ajH!0wBJ`ZL2p=iaJUfDO|sW)Qg;#Y3M&x#P^S$CzAJBbj_LmnFg?td z*=6#|g?#epN$JPsSGhAD9{Q{J^%|%1af7+#?dqk%MLDy~B{`W0^IHDjl`^{$f$9gB zXN**rTUYAaZq(^A@ZqdXEmCR}f+j+0l<4^b;kD6@nQ{q2g6Q(h*~P&SkJB=)?5xGv zGO;X0+KYVN&>u0nS4&@P=cePv6h=Ng9tA}%b0LC$S+8NToluv1FV9E9$X+TW$8Ie& zAN8XsGDs-}ZHni3FIGISb*)$$j%*kF-t>55hv_QRHRI4Ddt13!AS?T9+FhA8@_S?? z(FmdAi5~IUu7=6Rc0sK9Q6p;j_jY4rjy4HYl)dMg*Az>>r}eX#ezRK)AIy{~LR6Ub zh_*O6WpUdHn>oy+HUv-q#AS7pJ&}{Mu9z;`O=}Dz4i+;L|2>pauHy0Yl0v1l^{=EA zy8lYB50YSy8% zoRrc_33YX2<_gs!`_1oiYHqy=XefAmzpfWh8foa2>-esFp`|LQhe$idQP4@)eflUL z@tu+=km_{P4RtS8+-Y}L`ghRNj-s@*TMp|Vvi0+x-7HXq_(uo|eI=^4?;s%#zc^ic zSx?=!B1$LP(_m~2Us;Kim9`eYKzWljj)w8;WT^ki+Ner;-d!$@(VP&wE}EjXwZ2)G z>lMpb1t`?QR57;kJM*@kGe!KPaSp;_V#TZvn0tzTrjaW-%)E5z!s}R3mF+<(cqEE- zovnV#d_twX6aA(x?`1{Kn7WGPUzgviv`PwUWNxQJFtA=~!L^qtQO!zdznlyzRb^xx zj%Mk&&vN#M5`=4O2NxD9?bhu@!o!PrcCn1wf-=SUd%oXAF?tH&fb(3&kSD=*ucs)# zPI)xggZg4Oa&WsKr||W}mO69AgRYhDYO+kFA1^vz>CIX;4V&4f`EjN1M*Y{0C>h8p zh+Lg{bk%KgC|T4zJ!{8Os;w7;@BTETxmVe4I-9s}9HKbVhbC-_cu4ZUiK&q6MyGrgT)KXPgt}Ecw4kW! z>0#j~a4#x*My=<52XWMK`SRro-mGMcfEjnrt1r(-hOYFdSt`Zq;QK97oCmlOos7dE zAlVcxAU(H|Y~$p?1DRmqkw`m(0{`(GBvR?ld3+>N;3+IA66w`qoV`W5ZY^SU@v1(F zbWdD5da>VudS<3# ze^8~UU8@}5XB|XglA#1&sEp68thl&C`T6Ui-TPT@5A}cf@~*^h znrG>?cMT0s4{Ur~^tEsxh_ZR}W^#O_g||kMc1%Neq-DlHCe31OY|MMI<3?SSFwr>G z*xH(ki`#j68pE#h*rJS6)DBcjd-HanuZdMUe?@X$o<3a7x2Vi|1})U3mZF`ccj?2$ z8zL!BpQ=DBdxtuwy4C`EcLV+*tEiYxCa-#iVvEhOc*Kq7>$r@c99PGapti^6osU-a@|?ok6!hv^ zxqQWnw4@|8%OC7!y^?C@#E}sfcXxL&{yUoF-#fDW*^Joj0^H)t%9dFEUS3{_bSwbl zu%LzM=n+jzP1VFb7((A)3iTEaWH1~H+9EuZQD>3g-`}6-F=&bcJ`IZFfKU7MBXlih zvf17_iQc@8W zogvmHIqCX4^ucK50uV?XnYRuL_YVx{IXXVeF1fQvB9bne*-S5w#XYwgM+=0xT4;tVb7+I z(JTKFGyc`55bXZzAoI6AAipx}L3DIJiWxbX`4mcOLPED9F=l5E{N!u#ghk`KcM|V2 zs8kaegPJLIFi$@P?Hotppe^c{QZq6PU0p|?+9TRITNefDe{f?Q6Uzr_(LZyh^J#m< zz*v9N$qS98rGY2|%2u72Pcm-B9~lLORI8m)gSrvRXziH|4Z6=PiWQg%YeQCZq~TV< z`^TfCioQtS2@W36vSgDV1M&2nT6RGKI27bk!1B8JV+1 zVO*`Kka0tmX5!2PloLwbycfQj!y)m8ptm|I-(d5_syw-8YHG@}c!689`K6e8kF#eF zlVM}-=z}d@2$j}-W~@1_q{IS(aTd|BIoOih;~TJmbbx#Ph7FC#Il1OXw6h%dJsW@X z=8ZS{eRk`=nF?R+kadpOtAd_?aD+!h*q7bnEkvE|SXZb{ZA(+1TmRjCU+;++=P^9S^4pZe8dBElosv-sObTOm9XiWAT8?zRTWq+Er=ha)H(1Ewz8Q&$NA~aE?@HMr zArZzL)Smr1>?xz9^k`V&azb)(v3Xov+~}k4)z#Jdy1Ku=TSC(43$~IJmFLZRVUc<3 z_HFl1ua`)osDb?MpHaod=^Jc>!)msH86`T6W8UHxp^`;o;%dRB}xKhmU zo=CP9UCOKqCUu1&HavOqBv0G_(4j*}sw19?>(ZA!&k|X+1eYMQT zd^|NXQ|IPkmCFYXz-G&l5Kkz16DTW(=G1hNiQ}}SPF)k|Ate>K)s_M_$45lGSod@=dyc5BwDtBQhk~$Yq=7*KYSqpzevp}IwKMtm-&Lk&{52YG@rt{_)jp=JT|MYg$Gn8i@%r_z$dFym zawsj6MnWN8@8ttlFH0D*ejmS0&r~V(I244LeVdC#1?~RRFJ7fnt!!=8kpRz}IRo&g zuB9lbqomy}BDa5eheuI(Y;0_HVc=1d8ZQO@JKIClYaZ}Tcq-Zy_wQMCv&OV*5Svb3 zJ_Q)lSYpow%MkUmP1!GAD0X*ui>$DYz91THC zQ&Us7QvGht;kXmUK%rQyWNQ;_2?MN>lP6EQWTqlwhp}CX+EUZf^e$a0P}sN6 zz}Pssu&}Vpp=W@Zlis!G2ujovF_Q=!QsW~~iNC;ISz%L2a)tcC) zXN&qy?pCJy$69%K)8F*En+~j(`37 zT=Rv20xsTG;4P`i$?8fgJqn@?J$;m!Z3aqf#JoG0^2dSk7ND*#8bq{gvbWE%^2G11`e}i?Ybq>rQ<^WmL0lvsoOkxS=g7l-RA!C0m%3H3qv$ zr01U09PDAP42+NnVP1~QCyzQ={!vHb;}l7qUHC(1qk6Zvy5xG(Tu*MeQ%Jb z$50c{mqV9gOin&8VI}P7L)TIyqwigX^6bU7fFCmm+4dli4i0H3$cVdpmw*oDu|K}| zjG=D80Rb5BvLWWu#}wF?eFTxfbs~0HB6XcEX7}G08sf5?yz*)IdCAn93yJz$NmhLh z4i0+4A;*y@DO%LW8|0i1?EUC@rZqELf^J-StD;jZ|908wr3DLX*M`; z<L!*_OgDWYH ze*Jo}(uVs;8z*-CPe5MDAJgAEm1pDpM?zzJ(cNxS+dk;t}>VHR^UQ_FazjZG%fL^iln5aX{fg?Dw2#8}c`TtNt9s zo7&|=T2u5_D2Ssw-lDuTvPQ*#bnaMyhPb#m-9_|%xU1f;l9H0%T_I{R;J}X_3Hr?p zSn5n|e|5i_P6r%Fu$6h>`_*!`+mdzXg6|<^c6~5HG|$b$=Qb>oI?~~63|MAwZ?9?R zNdrH4X?48AS&o@7zcPoK_=5l}?d_SUW7~aqhp4>l6;TbS2~B(XnYA?nGP3P4&HIRV7v1{fQ)ttABoMj(v{Vq*l+|N7Mlpo8G$ zva+%mrB=$2y2#Uv45@3cPEp&Oo|)!rV(p4e#N55#a|caKCR)od5{N$YE|)%Km%Qyh zT#AcUISLWr6%D$zEngF(oiy*B!|O`U&2?=WzHhXwu+0fHs8(Ut^2eCv7WyM=NHyN5 zbHeY0WlxZ#H?6u!@H9KlbfnFh?>)*!XsKK%KEFnM`}PJ@FRU38n!$sr#+@+t?n`(O z6$Nv#27ZwCG1cWr%=|yMT;7no0A|DoR4(T zw(uQRC3GOcV4JGtS5Mo4DTRm4o^C8JKLRpz?y1PeIPgZdGG3$jF@7dRR!%PY`t^l? z75G9VSlqjE%oELBX`G{Z2!v#r6n~-itMn`uWuDK_6TV6*k?u6*7nNS1$uy8dpsr!@2ip~=v zt29!E+$78X2ulTgMtPnhQ*`Z0?2O=wo@3uiui3Kia`j26h+1tjl00X>FGOAaf!~c) zNsRlx{u$#*1u_2qPfAwZe7(-$skX9`WqVS)YFVG#7Mi-`yGwy>Nt);XHb{kcxBU;UikR&0c>@u_AZ{KRf8UM;+94@ zIBLYux#683>vZ<}^iG?BBK#J(PLAU~`52+4*SbchbP2q}0Ud%sj5;jM z_@oq2^Q3QMll0MhKoWHOnQ^wTy!7fjN5*RR4JAW*i?B|C02!iw*wN9ksyKB!pld&U z&?7A)LmLkb-IG}Q*kR<%n9K|G95Qol@zm#N;6{6}nG8C?2#zh%Ok$(}2|{=jQOhxt zio{jA-4sFx6V)0c=E0QVgg9f0oB^&p^kQH!UJ^E0xuc8o2_g<=X2y<2L2K62W2E=s zqc+!4^{2`4WHPxcnPK*MV4t#bz(@CUTw{oH4q1tZ3@==`kW+rYqn|ZSy$hLL^rtjj zix-s?cp7gT=Q}t9yuU z@GFTF{{(Y@(#a|$-hKV-rj~OMow4dvAOa%| zjViLqfCh2hV_KpMHoC&;`0vI0e*@V4RQvz``GBkS=$p=A&pqve&?q;refc-6j-P|4 ztCwPTu5Ke-h{&5CPAghvdu@Y0E1NiadpPF9nd~S?%}`k(x=PSlC!v)-+|8sQ8RFaE zRrL^D(?LhZCMLuY2>RR}e8m%#MtLe`;0r&9BeYu(GNjtXB8ZWd>&#REpb01F?~0@6 zt2oouMT8O8amLQ*h(9n~D=mN~l7f>-fn9pK{WwUa*XWblU)SV*{(NrAx8!^zI7CiR zp!MZzfW5F>B$SlvQu8P;X4;>QAubwn@^0qvUhP4JdClcLf;Q$z8Jan6S#>Y z*C@`S(S869N)wvw$5#CKwu1ekM=1&C!yTDFe|}-d_4Ta$Ouu77QmophK1Y?S^uC(j#xpAMDH@scS4e4??D6*eH$OS8bdCF78F= zf8*{t#W)m3_TNEi2y+nH{M@I8e0%nRRTiR2gLQ3(??D1Z@_qLS3JOvu6GsxD;!zXL z|I`fHtT*vyE-w;1F2TnDp_BofSnu@wphf0{z>g&eJ6xCRR}u^vhz%vO1meNUGDRij z$RzHWjPR^QR+^yJ*?~gf5T7stW830EiNSWcDYyft^!g3?#LfbMAEM0+UKm4Po-La4 zwr|_^7F2(XTIZ)tsMR8Nkg(Y`HcD~uevp{jTGXlVUq?x?OTvLcPL&O4LbfKtBLDD%*3N7 z5ZrNANC2kFy`Z~R5W%$xfqE9xY>M>3E?uPbIMcUVyz00C1e){Fv9X!p2FMD%AOZ@` ze*{XQ+qMgP<07H*z-uDrF5JlCzTzHeTPT#(A_E~BfI%u68W}};_~8^df_`OYDtF2b zdzx8JRU5HzxScJMG87IZSORWB^m-8x46zQ_r)x+sSPkFj7ZPelS84h;`O#h2P>$tm zsO@NfE=1}gaG&yYt}?@j#x|H~YoCb`Nx?~ZEl$wjfmaEI79-6lVcXAO9Vy1q+P{C^ zyB%0J(|=4{UciR?IdT^sX~)zwPK?S;W&;-ju|_Pr${>n?vF(iN6i0)IwGFO}LD4CL zd9zZxdHMJVzJR0(luwp%*w0Tq*pZ~g%+^+%KA?mo&&$gzn)E(#KRTWiJik4Lu#Fzx zXYJF)?Ru)J*Kn*x3K139!@&XRkbVd_J`zcLht8xt{~7ORCb>t@-uJk?ygW=@+7A~t_YYs1Z1euP=Hm09gw2qr@3N3>mC8SfiF*X#4x^HwUDl*OiX#Y)bPlR>Dq(HD%r8CX9JEH@r2Nv25Yh%t!R~e&e~Z zz#gGaXh3%DJ6Fa2dWn{X)3N$sI9gD&5TI=!V`P$9ttx{NAD+3ms6w75(1At+cLf^ARlCN~end`ORXb()ZLOu zZZ4`@T)6&+#{Rz{dAV8_AsstAwKAE6%sOY=Nmma|q{YV{#w%};vOY*4H5X%0L^v5B z>J%*v=080&2s-B9HlZ?&@W0ltXUzFuODz{5iX`MEnv*yg%Z(@~{&^7ESZM=@8r=XyKHU98>TZeP+{vj`& zk%^9uQ|^~85m^H77+T#41km`DfOa1rA5je?UIZYr2spO8o0~0ovWr|zP<@T!3S{V7 z)10?hXiN(4;srr3ny126a}dq6ge`@e=9n+d5;-iqau7XZ&jJajhB&|g_kcH5dKBt` zDNf{iI-lrW{_H#WuPho74IJ_K4o7u@a>7GO(UPKyOnrwh5_C={X;Lx$Q5 z($U&$kA7sVQh8a$#b(Kn0!0&O&-1dgPb4HH+~2mO)I?yvH}ve^bJFEs9sjSHwaYQp z`S8Y(Ji)Fs^m>-lbAke@9w@Dt0-N4HnosA2vy8!YO-y4wMGWc&if$4N-YS&j(ty1| z?)uYCAwBFfn`JhGM?o$EbxH;;Sa(-Aq%ys4_MhfS&uCSLvn^&GJPm3*f=K_@uZA$6 z=*g2!#Op!FDTYr7 zFJp!btSWE`krWEW5Y{iAj=)yvW6%c|L;wdJHzViaG?L8@(o2j@ShEu}%O6v*KT>{@ zX^ABahpYzTATiWI_Jq{+oMa{AZ!M(^Bx%T-#2gHJ!iFNBvNr{B*51m;$G4^V3@9c+ z6VVM(kMoR9Ldw}D7(A!OK_eAbZW;O|D~Hf~(aWw!p_3wl5P#t*^aic|((-pN$d&#bz3u=qdTfeQC3U}Mu3xuMd&$%YJvv3 zF54)uDyYN6&_<_NC^sRnic^7)p)z(7vZ@~VXncy@Yg-wG9-dAr(9m%mow{|K=-nr4NW);JBhNMGHtSw1NH_{kP%gV*ZrK;&qo*bO36`NnG`h`^rSZDwY=j+S9NW{9* zd7qaa-CaP|NSL|y+gxy@kL?#`EIGUfLPnR*F2{@a=m z^DO5{kD+Za=2)_HpgKM%MB6?r)lCK1+b zNu-zB`;JUhl;hjk#x=I>L%ZL(%5{YvOJt2;V&tH5CK0b4rDmnOAqX9j!e4vH}Ja5mZ%Gm1p^CG4ZPH&S`T= z&+*aH8oPr(t*0n?$w~r6lnTc7-W&h` literal 0 HcmV?d00001 diff --git a/rqt_joint_trajectory_controller/doc/userdoc.rst b/rqt_joint_trajectory_controller/doc/userdoc.rst new file mode 100644 index 0000000000..197a7afb30 --- /dev/null +++ b/rqt_joint_trajectory_controller/doc/userdoc.rst @@ -0,0 +1,12 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/rqt_joint_trajectory_controller/doc/userdoc.rst + +.. _rqt_joint_trajectory_controller_userdoc: + +rqt_joint_trajectory_controller +=============================== + +rqt_joint_trajectory_controller is a GUI plugin for rqt that allows to command a joint_trajectory_controller. + +.. image:: rqt_joint_trajectory_controller.png + :width: 400 + :alt: rqt_joint_trajectory_controller From d915497394710481a98cbaf7b6db8ef9a368aa3e Mon Sep 17 00:00:00 2001 From: Franz Rammerstorfer <60260174+franzrammerstorfer@users.noreply.github.com> Date: Sat, 30 Dec 2023 21:25:35 +0100 Subject: [PATCH 37/41] Fix ackermann steering odometry (#921) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix Ackermann steering odometry bug issue #878 --------- Co-authored-by: Christoph Fröhlich --- steering_controllers_library/CMakeLists.txt | 3 + .../src/steering_odometry.cpp | 4 +- .../test/test_steering_odometry.cpp | 110 ++++++++++++++++++ 3 files changed, 115 insertions(+), 2 deletions(-) create mode 100644 steering_controllers_library/test/test_steering_odometry.cpp diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index b64ea204a8..63de42cf69 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -67,6 +67,9 @@ if(BUILD_TESTING) controller_interface hardware_interface ) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) + target_link_libraries(test_steering_odometry steering_controllers_library) + endif() install( diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index bf254bfcfa..e2ced036ff 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -270,8 +270,8 @@ std::tuple, std::vector> SteeringOdometry::get_comma double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); double denominator_second_member = wheel_track_ * std::sin(alpha); - double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp new file mode 100644 index 0000000000..173c76baef --- /dev/null +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2023, Virtual Vehicle Research 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 "gmock/gmock.h" + +#include "steering_controllers_library/steering_odometry.hpp" + +TEST(TestSteeringOdometry, initialize) +{ + EXPECT_NO_THROW(steering_odometry::SteeringOdometry()); + + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 3.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + EXPECT_DOUBLE_EQ(odom.get_heading(), 0.); + EXPECT_DOUBLE_EQ(odom.get_x(), 0.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(2., 0., 0.5); + EXPECT_DOUBLE_EQ(odom.get_linear(), 2.); + EXPECT_DOUBLE_EQ(odom.get_x(), 1.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), 1.); + + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., -1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_LT(odom.get_y(), 0); // neg y ie. right +} + +TEST(TestSteeringOdometry, ackermann_back_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0.); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], cmd1[1]); // no steering + EXPECT_EQ(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner) + EXPECT_GT(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); +} From f14396cc14d023a8b9d6bc9a38331e4af59f181e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 2 Jan 2024 16:14:10 +0100 Subject: [PATCH 38/41] [JTC] Cleanup includes (#943) --- .../joint_trajectory_controller.hpp | 18 +++--------------- .../joint_trajectory_controller/tolerances.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 8 +------- .../test/test_trajectory_controller.cpp | 5 ----- .../test/test_trajectory_controller_utils.hpp | 1 - 5 files changed, 4 insertions(+), 31 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 d16e4f8267..0ed2ce5688 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 @@ -16,10 +16,9 @@ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #include +#include // for std::reference_wrapper #include -#include #include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" @@ -30,15 +29,15 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_action/types.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.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_server_goal_handle.h" @@ -50,19 +49,8 @@ using namespace std::chrono_literals; // NOLINT -namespace rclcpp_action -{ -template -class ServerGoalHandle; -} // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - namespace joint_trajectory_controller { -class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b5e660be54..792938b96f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,9 +30,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ -#include -#include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c6a5169855..b1cca67547 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -14,12 +14,10 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include #include #include #include -#include -#include + #include #include @@ -33,15 +31,11 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..72820c0295 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -30,7 +29,6 @@ #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -44,9 +42,6 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7b68d882ff..e636fad4b7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,7 +25,6 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "joint_trajectory_controller/trajectory.hpp" namespace { From 2b52f0b54168341b2afe94f0aeef92c5fd41381a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jan 2024 10:05:31 +0100 Subject: [PATCH 39/41] Add few warning flags to error (#961) * add conversion, unused-but-set-variable, format, and return-type warnings to error * Add fixes to the added flags * fix linters --- ackermann_steering_controller/CMakeLists.txt | 3 ++- admittance_controller/CMakeLists.txt | 3 ++- bicycle_steering_controller/CMakeLists.txt | 3 ++- diff_drive_controller/CMakeLists.txt | 2 +- effort_controllers/CMakeLists.txt | 3 ++- force_torque_sensor_broadcaster/CMakeLists.txt | 3 ++- forward_command_controller/CMakeLists.txt | 3 ++- gripper_controllers/CMakeLists.txt | 2 +- imu_sensor_broadcaster/CMakeLists.txt | 3 ++- joint_state_broadcaster/CMakeLists.txt | 3 ++- joint_trajectory_controller/CMakeLists.txt | 2 +- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- pid_controller/CMakeLists.txt | 2 +- pid_controller/src/pid_controller.cpp | 4 ++-- position_controllers/CMakeLists.txt | 3 ++- range_sensor_broadcaster/CMakeLists.txt | 2 +- steering_controllers_library/CMakeLists.txt | 3 ++- tricycle_controller/CMakeLists.txt | 3 ++- tricycle_steering_controller/CMakeLists.txt | 3 ++- velocity_controllers/CMakeLists.txt | 3 ++- 20 files changed, 34 insertions(+), 21 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 6ad0e9485f..66f09c5f09 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a8a8832fce..b2c10e0ba5 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 5d662c1fec..7118e9a44d 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..436832c523 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,7 @@ 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 -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index ad97690655..eae8642fb6 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 2af5500e21..38c984192e 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 15ffe09750..4885c69c8a 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 8edaaf6386..676062f7a3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 3b09d9e72e..6782012c65 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cadc96b4e3..5c383897cc 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + 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 diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 88bb7891a3..5cddb1bf4d 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,7 @@ 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 -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 792938b96f..2157fdf2a6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %ld:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 81cbe6f006..15e903222a 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 373e941d96..05fee986dd 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -171,10 +171,10 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void + [&](const std::shared_ptr state_msg) -> void { // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(msg); + measured_state_.writeFromNonRT(state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 54a4f94656..18f3cb313a 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 00da395db5..d7002d1b17 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 63de42cf69..88f24d304c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 291f08ad9f..4f6d064dc8 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 4c56d68185..02c9453ace 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 3642ae1cb9..a39cd162fd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + 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 From 7d0e1d1a38d902146bd3771a82f39ebaed51cb16 Mon Sep 17 00:00:00 2001 From: maurice Date: Sun, 7 Jan 2024 09:03:14 +0100 Subject: [PATCH 40/41] Update deprecated topic name (#964) --- joint_trajectory_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6fe2146802..af495ad14d 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -168,7 +168,7 @@ Subscriber [#f1]_ The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held. -Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. +Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/controller_state`` topic it is much more cumbersome to realize than with the action interface. Publishers From 30eb4b07d630f4b4b5be6080a6ae9469bdab2f18 Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Sun, 7 Jan 2024 01:04:35 -0700 Subject: [PATCH 41/41] Remove robot description param from admittance YAML (#963) --- .../src/admittance_controller_parameters.yaml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index ee1efa67ab..315d0e70d2 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -147,11 +147,6 @@ admittance_controller: } # general settings - robot_description: { - type: string, - description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", - read_only: true - } enable_parameter_update_without_reactivation: { type: bool, default_value: true,