From c831b6cee52886bf3e93300b63f7b8d9e3566bb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Oct 2023 12:58:44 +0200 Subject: [PATCH 01/19] Update requirements of state interfaces (#798) --- joint_trajectory_controller/doc/userdoc.rst | 10 ++++++++-- .../validate_jtc_parameters.hpp | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index deedc4a7b6..58ba734b45 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -17,7 +17,7 @@ Waypoints consist of positions, and optionally velocities and accelerations. Hardware interface types ------------------------------- -Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations: +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations as command interfaces: * ``position`` * ``position``, ``velocity`` @@ -37,10 +37,16 @@ This leads to the following allowed combinations of command and state interfaces * With command interface ``velocity``: * if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` . - * no restrictions otherwise. * With command interface ``effort``, state interfaces must include ``position, velocity``. +* With command interface ``acceleration``, state interfaces must include ``position, velocity``. + +Further restrictions of state interfaces exist: + +* ``velocity`` state interface cannot be used if ``position`` interface is missing. +* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present." + Example controller configurations can be found :ref:`below `. Other features diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 5a656e7754..0a6bf99bb7 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -41,7 +41,7 @@ tl::expected command_interface_type_combinations( { return tl::make_unexpected( "'velocity' command interface can be used either alone or 'position' " - "interface has to be present"); + "command interface has to be present"); } if ( @@ -51,7 +51,7 @@ tl::expected command_interface_type_combinations( { return tl::make_unexpected( "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); + "'position' command interfaces are present"); } if ( From ac291ab511545074564155f361673d8301550d74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Oct 2023 13:00:53 +0200 Subject: [PATCH 02/19] Steering controllers library: fix open loop mode (#793) * set last*velocity variables for open loop odometry * Make function arguments const * Update function in header file too --- .../steering_controllers_library/steering_odometry.hpp | 3 ++- .../src/steering_controllers_library.cpp | 7 ++++--- steering_controllers_library/src/steering_odometry.cpp | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 3cfa056b27..95bcef7e63 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -184,7 +184,8 @@ class SteeringOdometry * \param theta_dot Desired angular velocity [rad/s] * \return Tuple of velocity commands and steering commands */ - std::tuple, std::vector> get_commands(double Vx, double theta_dot); + std::tuple, std::vector> get_commands( + const double Vx, const double theta_dot); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index af2736a8a3..eb497ebb3d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -443,10 +443,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { // store and set commands - const double linear_command = reference_interfaces_[0]; - const double angular_command = reference_interfaces_[1]; + last_linear_velocity_ = reference_interfaces_[0]; + last_angular_velocity_ = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = - odometry_.get_commands(linear_command, angular_command); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0a7dc23ef2..bf254bfcfa 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -210,7 +210,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub } std::tuple, std::vector> SteeringOdometry::get_commands( - double Vx, double theta_dot) + const double Vx, const double theta_dot) { // desired velocity and steering angle of the middle of traction and steering axis double Ws, alpha; From 22356e0e9d39c726926f3f3a0ddb7afd51333295 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 16 Oct 2023 18:49:47 +0200 Subject: [PATCH 03/19] [TestNodes] Optimize output about setup of JTC publisher (#792) --- .../publisher_forward_position_controller.py | 4 ++-- .../publisher_joint_trajectory_controller.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 59a39164ec..5cf28ac604 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -46,8 +46,8 @@ def __init__(self): self.goals.append(float_goal) self.get_logger().info( - f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ - every {wait_sec_between_publish} s' + f"Publishing {len(goal_names)} goals on topic '{publish_topic}' " + f"every {wait_sec_between_publish} s'" ) self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 4ed198515e..cb66f58468 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -129,8 +129,8 @@ def get_sub_param(sub_param): publish_topic = "/" + controller_name + "/" + "joint_trajectory" self.get_logger().info( - f'Publishing {len(goal_names)} goals on topic "{publish_topic}" every ' - "{wait_sec_between_publish} s" + f"Publishing {len(goal_names)} goals on topic '{publish_topic}' every " + f"{wait_sec_between_publish} s" ) self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) From b36b9d20f186f50ce9261aa8a55fb0d28f849959 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 24 Oct 2023 20:17:28 +0200 Subject: [PATCH 04/19] [CI] Fix coverage build and codecov stats (#804) --- .github/workflows/ci-coverage-build.yml | 18 +++++++++++++++++- codecov.yml | 2 ++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index d2dba1bce8..8ca9b968d3 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -27,9 +27,25 @@ jobs: with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed in the meta package + # build all packages listed here package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + position_controllers + range_sensor_broadcaster + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers vcs-repo-file-url: | https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} diff --git a/codecov.yml b/codecov.yml index 02deea2321..ca8c1cc0ac 100644 --- a/codecov.yml +++ b/codecov.yml @@ -9,6 +9,8 @@ coverage: patch: off fixes: - "ros_ws/src/ros2_controllers/::" +ignore: + - "**/test" comment: layout: "diff, flags, files" behavior: default From b65314d6fcb0d57b3c86e7bf3b05baa4ccdb984f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 26 Oct 2023 12:09:27 +0200 Subject: [PATCH 05/19] Cleanup comments and unnecessary checks (#803) --- .../joint_trajectory_controller.hpp | 6 ++---- .../src/joint_trajectory_controller.cpp | 18 +----------------- 2 files changed, 3 insertions(+), 21 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 b3e2f55742..0366c8e6d6 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 @@ -71,15 +71,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JointTrajectoryController(); /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e39bffc73..9e0b4823f3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -651,17 +651,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - // Check if the DoF has changed - if ((dof_ > 0) && (params_.joints.size() != dof_)) - { - RCLCPP_ERROR( - logger, - "The JointTrajectoryController does not support restarting with a different number of DOF"); - // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we - // can continue - return CallbackReturn::FAILURE; - } - + // get degrees of freedom dof_ = params_.joints.size(); // TODO(destogl): why is this here? Add comment or move @@ -691,12 +681,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - // // Specialized, child controllers set interfaces before calling configure function. - // if (command_interface_types_.empty()) - // { - // command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - // } - if (params_.command_interfaces.empty()) { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); From 8a90b5143669c61ec8eded494cc8c75bbb6c07e5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 31 Oct 2023 20:27:04 +0000 Subject: [PATCH 06/19] Bump ros-tooling/action-ros-ci from 0.3.3 to 0.3.4 (#810) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 8ca9b968d3..d33ee89cc9 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -23,7 +23,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.3 + - uses: ros-tooling/action-ros-ci@0.3.4 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 167386ceff..ad3dcd505d 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.3 + - uses: ros-tooling/action-ros-ci@0.3.4 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From 822552fb700885ca8a821651c13eb5263e2c148c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 31 Oct 2023 21:33:40 +0000 Subject: [PATCH 07/19] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 9 +++++++++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 20 files changed, 78 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index d576811bdb..171dd78b5f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Improve docs (`#785 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-09-20) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index e17d56865d..077d31f1de 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 7c652e7fa7..aed3400b0a 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Improve docs (`#785 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-09-20) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index d670ccc047..0369169b8f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index efc583475d..5b36e548a5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 70fe56b159..0d9dc0f8dd 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 +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index ca209a52f5..7cefd99297 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bc66422a37..3b2a414598 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 477134c496..fcda055f9d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 35298737f0..71df1d5728 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a322fef5ad..e294291f02 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup comments and unnecessary checks (`#803 `_) +* Update requirements of state interfaces (`#798 `_) +* [JTC] Add tests for acceleration command interface (`#752 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-09-20) ------------------- * [Docs] Improve interface description of JTC (`#770 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 06e2f3e8db..b0492117d2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 9d5242684a..499a28868d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index d21abd09ca..012264e6e6 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 525d0a0221..07b540f28a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [TestNodes] Optimize output about setup of JTC publisher (`#792 `_) +* Contributors: Dr. Denis + 3.16.0 (2023-09-20) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ecf0568408..6c7f06200c 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.16.0 (2023-09-20) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index a7546cb59d..e3b9d163af 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Steering controllers library: fix open loop mode (`#793 `_) + * set last*velocity variables for open loop odometry + * Make function arguments const + * Update function in header file too +* Improve docs (`#785 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-09-20) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 91175391f3..5cad2f94f7 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index dd50c98185..a4645ebfb5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Improve docs (`#785 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-09-20) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ce8bcfefa3..0b8c320035 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.16.0 (2023-09-20) ------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) From 6e4605316ab63c8407f58f40884bebc90ca0b2be Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 31 Oct 2023 21:34:10 +0000 Subject: [PATCH 08/19] 3.17.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 171dd78b5f..d679a782d4 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- * Improve docs (`#785 `_) * Contributors: Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index f866089f1c..656c88feae 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.16.0 + 3.17.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 077d31f1de..ba5a131094 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 43a7ab0aad..62cf127d9f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.16.0 + 3.17.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 aed3400b0a..7b14b2d617 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- * Improve docs (`#785 `_) * Contributors: Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 8707502915..9ee4c53049 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.16.0 + 3.17.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 0369169b8f..b3f4b127e3 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 611dfbe15a..01dc69413a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.16.0 + 3.17.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 5b36e548a5..baaada7c22 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 9e3cc8bc32..d0627bea10 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.16.0 + 3.17.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 0d9dc0f8dd..9b6dea8ef9 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 ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 12ec3d1945..81c865ddfb 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.16.0 + 3.17.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 7cefd99297..4bc9d41c74 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c8d0b99a59..246bc6777d 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.16.0 + 3.17.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3b2a414598..b7c7242520 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index dcd778363e..391bd1bb7d 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.16.0 + 3.17.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index fcda055f9d..8644b008a0 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6173569630..2dcacc1f0f 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.16.0 + 3.17.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 71df1d5728..fd56b72c3b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 05f761169a..e4fecb14d7 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.16.0 + 3.17.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 e294291f02..9eece3af21 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- * Cleanup comments and unnecessary checks (`#803 `_) * Update requirements of state interfaces (`#798 `_) * [JTC] Add tests for acceleration command interface (`#752 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index d8f2e8e997..f4efc2c060 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.16.0 + 3.17.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 b0492117d2..5239c43407 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 712d03068a..b3cd9b6cfb 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.16.0 + 3.17.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 499a28868d..b1084b4a24 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 70d9c1fa2f..cf6545d65e 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.16.0 + 3.17.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 012264e6e6..187db3d351 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 35cbdbf9bd..49c87b152d 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.16.0 + 3.17.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 07b540f28a..acca28227f 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 ------------ +3.17.0 (2023-10-31) +------------------- * [TestNodes] Optimize output about setup of JTC publisher (`#792 `_) * Contributors: Dr. Denis diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 398a279ae7..ef8c7b572d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.16.0 + 3.17.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 9b4e3a8b6b..102c39b607 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.16.0", + version="3.17.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 6c7f06200c..e723d39c6f 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 ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 96b34a5609..08f885aef3 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.16.0 + 3.17.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 cf09679e00..c0f4a96c2a 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.16.0", + version="3.17.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 e3b9d163af..8082dc9070 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- * Steering controllers library: fix open loop mode (`#793 `_) * set last*velocity variables for open loop odometry * Make function arguments const diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index c7c654846a..0194cfd041 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.16.0 + 3.17.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 5cad2f94f7..bda1c89d32 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index ec4d5e064d..9a6cf3ef2d 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.16.0 + 3.17.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 a4645ebfb5..a7e6854ea8 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- * Improve docs (`#785 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 64ebddd5e2..e39971982c 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.16.0 + 3.17.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 0b8c320035..358ee9f499 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.17.0 (2023-10-31) +------------------- 3.16.0 (2023-09-20) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e47e170226..f336606b26 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.16.0 + 3.17.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From c6ecab7e14f6f2935a642f742f03142e1d18c9b2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 1 Nov 2023 21:38:16 +0000 Subject: [PATCH 09/19] Use pre-commit fork of clang-format instead of local (#813) --- .pre-commit-config.yaml | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5a7ac74d9b..32d194d73d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -58,21 +58,10 @@ repos: args: ["--ignore=E501"] # CPP hooks - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] - # The same options as in ament_cppcheck are used, but its not working... - #- repo: https://github.com/pocc/pre-commit-hooks - #rev: v1.1.1 - #hooks: - #- id: cppcheck - #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - repo: local hooks: From f81a82c9d73f9fc06aeec3b031a1c63dc144ebdd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 3 Nov 2023 21:18:31 +0100 Subject: [PATCH 10/19] [CI] Codecov (#807) --- .github/workflows/ci-coverage-build.yml | 2 -- README.md | 3 +++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index d33ee89cc9..694f3dd8a1 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -1,8 +1,6 @@ name: Coverage Build on: workflow_dispatch: - branches: - - master push: branches: - master diff --git a/README.md b/README.md index 8c8539a02b..49d6b28ea0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ # ros2_controllers +[![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. ## Build status From 2c6d7a6b83e905786490a328967a87460f2c52b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:29:10 +0000 Subject: [PATCH 11/19] Add iron branch (#814) --- .github/mergify.yml | 10 +++++++ ...ibility.yml => iron-abi-compatibility.yml} | 8 ++--- ...ld-main.yml => iron-binary-build-main.yml} | 18 +++++++----- ...ting.yml => iron-binary-build-testing.yml} | 18 +++++++----- ...y-build.yml => iron-rhel-binary-build.yml} | 17 ++++------- ...in.yml => iron-semi-binary-build-main.yml} | 18 +++++++----- ...yml => iron-semi-binary-build-testing.yml} | 18 +++++++----- ...source-build.yml => iron-source-build.yml} | 12 ++++---- .github/workflows/prerelease-check.yml | 2 ++ ros2_controllers.iron.repos | 29 +++++++++++++++++++ 10 files changed, 101 insertions(+), 49 deletions(-) rename .github/workflows/{humble-abi-compatibility.yml => iron-abi-compatibility.yml} (78%) rename .github/workflows/{humble-binary-build-main.yml => iron-binary-build-main.yml} (61%) rename .github/workflows/{humble-binary-build-testing.yml => iron-binary-build-testing.yml} (61%) rename .github/workflows/{humble-rhel-binary-build.yml => iron-rhel-binary-build.yml} (65%) rename .github/workflows/{humble-semi-binary-build-main.yml => iron-semi-binary-build-main.yml} (59%) rename .github/workflows/{humble-semi-binary-build-testing.yml => iron-semi-binary-build-testing.yml} (59%) rename .github/workflows/{humble-source-build.yml => iron-source-build.yml} (66%) create mode 100644 ros2_controllers.iron.repos diff --git a/.github/mergify.yml b/.github/mergify.yml index 49d2acedfa..3aaaab2001 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,6 +8,16 @@ pull_request_rules: branches: - humble + + - name: Backport to iron at reviewers discretion + conditions: + - base=master + - "label=backport-iron" + actions: + backport: + branches: + - iron + - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml similarity index 78% rename from .github/workflows/humble-abi-compatibility.yml rename to .github/workflows/iron-abi-compatibility.yml index 708ea5c1f4..20d93f5af1 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -1,11 +1,11 @@ -name: Humble - ABI Compatibility Check +name: Iron - ABI Compatibility Check on: workflow_dispatch: branches: - - humble + - iron pull_request: branches: - - humble + - iron jobs: abi_check: @@ -14,7 +14,7 @@ jobs: - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: - ROS_DISTRO: humble + ROS_DISTRO: iron ROS_REPO: main ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml similarity index 61% rename from .github/workflows/humble-binary-build-main.yml rename to .github/workflows/iron-binary-build-main.yml index 64d78f281a..ef35397855 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -1,17 +1,21 @@ -name: Humble Binary Build - main +name: Iron Binary Build - main # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: workflow_dispatch: branches: - - humble + - iron + - '*feature*' + - '*feature/**' pull_request: branches: - - humble + - iron + - '*feature*' + - '*feature/**' push: branches: - - humble + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -20,7 +24,7 @@ jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: - ros_distro: humble + ros_distro: iron ros_repo: main - upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers-not-released.iron.repos + ref_for_scheduled_build: iron diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml similarity index 61% rename from .github/workflows/humble-binary-build-testing.yml rename to .github/workflows/iron-binary-build-testing.yml index 524cacd685..25a693dc23 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -1,17 +1,21 @@ -name: Humble Binary Build - testing +name: Iron Binary Build - testing # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' on: workflow_dispatch: branches: - - humble + - iron + - '*feature*' + - '*feature/**' pull_request: branches: - - humble + - iron + - '*feature*' + - '*feature/**' push: branches: - - humble + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -20,7 +24,7 @@ jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: - ros_distro: humble + ros_distro: iron ros_repo: testing - upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers-not-released.iron.repos + ref_for_scheduled_build: iron diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml similarity index 65% rename from .github/workflows/humble-rhel-binary-build.yml rename to .github/workflows/iron-rhel-binary-build.yml index 9da6059892..5664d61768 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -1,26 +1,21 @@ -name: Humble RHEL Binary Build +name: Iron RHEL Binary Build on: workflow_dispatch: - branches: - - humble - pull_request: - branches: - - humble push: branches: - - humble + - iron schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' jobs: - humble_rhel_binary: - name: Humble RHEL binary build + iron_rhel_binary: + name: Iron RHEL binary build runs-on: ubuntu-latest env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-rhel + ROS_DISTRO: iron + container: ghcr.io/ros-controls/ros:iron-rhel steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml similarity index 59% rename from .github/workflows/humble-semi-binary-build-main.yml rename to .github/workflows/iron-semi-binary-build-main.yml index 863df79a22..2224a59f0e 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -1,16 +1,20 @@ -name: Humble Semi-Binary Build - main +name: Iron Semi-Binary Build - main # description: 'Build & test that compiles the main dependencies from source.' on: workflow_dispatch: branches: - - humble + - iron + - '*feature*' + - '*feature/**' pull_request: branches: - - humble + - iron + - '*feature*' + - '*feature/**' push: branches: - - humble + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -19,7 +23,7 @@ jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: - ros_distro: humble + ros_distro: iron ros_repo: main - upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml similarity index 59% rename from .github/workflows/humble-semi-binary-build-testing.yml rename to .github/workflows/iron-semi-binary-build-testing.yml index 6286636e1f..c5ff430c89 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -1,16 +1,20 @@ -name: Humble Semi-Binary Build - testing +name: Iron Semi-Binary Build - testing # description: 'Build & test that compiles the main dependencies from source.' on: workflow_dispatch: branches: - - humble + - iron + - '*feature*' + - '*feature/**' pull_request: branches: - - humble + - iron + - '*feature*' + - '*feature/**' push: branches: - - humble + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -19,7 +23,7 @@ jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: - ros_distro: humble + ros_distro: iron ros_repo: testing - upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/iron-source-build.yml similarity index 66% rename from .github/workflows/humble-source-build.yml rename to .github/workflows/iron-source-build.yml index ff0fd62e05..1e9d865c49 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -1,11 +1,11 @@ -name: Humble Source Build +name: Iron Source Build on: workflow_dispatch: branches: - - humble + - iron push: branches: - - humble + - iron schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -14,6 +14,6 @@ jobs: source: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: - ros_distro: humble - ref: humble - ros2_repo_branch: humble + ros_distro: iron + ref: iron + ros2_repo_branch: iron diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 856d877d85..182df6e22b 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -10,6 +10,7 @@ on: type: choice options: - humble + - iron - rolling branch: description: 'Chose branch for distro' @@ -18,6 +19,7 @@ on: type: choice options: - humble + - iron - master jobs: diff --git a/ros2_controllers.iron.repos b/ros2_controllers.iron.repos new file mode 100644 index 0000000000..4e2c7abbbd --- /dev/null +++ b/ros2_controllers.iron.repos @@ -0,0 +1,29 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: iron + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + angles: + type: git + url: https://github.com/ros/angles.git + version: ros2 + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From 5f78fe40bd2573ce3d3da6c22c6b4e69897a7a4d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Nov 2023 12:01:02 +0000 Subject: [PATCH 12/19] Adjust tests after passing URDF to controllers (#817) --- .../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 | 36 ++++++++++--------- .../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 | 3 +- .../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 | 16 +++++---- .../test_tricycle_steering_controller.hpp | 2 +- .../test_joint_group_velocity_controller.cpp | 2 +- 18 files changed, 44 insertions(+), 41 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 80258258c2..8af2f6bc94 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, ""), 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 8888cd700a..db708db6c5 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, "", "", 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 065f9e1a0d..34d6883a83 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, ""), 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 edaaa84e4b..505aa44d2b 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -189,11 +189,13 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; + + const std::string urdf_ = ""; }; TEST_F(TestDiffDriveController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -201,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -221,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -237,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -257,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -290,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -325,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -361,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, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -396,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, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -433,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, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -467,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -481,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -497,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -514,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -531,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -548,7 +550,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -597,7 +599,7 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); 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 256a4ce465..23555c578f 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", ""); 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 57650c9302..82f677410e 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", ""); 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 b39294f36c..2ab507ef29 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", ""); 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 c629b36ba8..00ca4ae2d1 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", ""); 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 506f968b99..572e755743 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", ""); 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 8358088b1d..0b782efc5f 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -37,7 +37,6 @@ namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; - } // namespace void IMUSensorBroadcasterTest::SetUpTestCase() {} @@ -54,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", ""); 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 7f95e232e2..a6c0708fd9 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", ""); 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 cea61c92bf..7c65fec29c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -189,7 +189,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_, "", "", 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 a712cc81c2..07f42e4eba 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", ""); 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 855f7e037b..ab937146cb 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, ""); 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 c72c61257a..226d4f4291 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, ""), 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 1d6edf261a..054fff10ce 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -167,18 +167,20 @@ class TestTricycleController : public ::testing::Test rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; + + const std::string urdf_ = ""; }; TEST_F(TestTricycleController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); 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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -198,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -211,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -225,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); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -241,7 +243,7 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -290,7 +292,7 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name); + const auto ret = controller_->init(controller_name, urdf_); 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 422f399ad4..e9d1023d98 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, ""), 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 43c1545461..185c630bc9 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", ""); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From 9d4ea6b7cf7476e21e690a5c05e8f7339c8819b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Nov 2023 12:01:23 +0000 Subject: [PATCH 13/19] [diff_drive_controller] Remove non-stamped Twist option (#812) --- diff_drive_controller/doc/userdoc.rst | 3 - .../diff_drive_controller.hpp | 3 - .../src/diff_drive_controller.cpp | 64 ++++++------------- .../src/diff_drive_controller_parameter.yaml | 5 -- 4 files changed, 18 insertions(+), 57 deletions(-) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index f8318bc4b8..d2dd284cf3 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -48,9 +48,6 @@ Subscribers ~/cmd_vel [geometry_msgs/msg/TwistStamped] Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. -~/cmd_vel_unstamped [geometry_msgs::msg::Twist] - Velocity command for the controller, if ``use_stamped_vel=false``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. - Publishers ,,,,,,,,,,, diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 5923a27d4d..554bedba59 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -127,8 +127,6 @@ class DiffDriveController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; @@ -151,7 +149,6 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; bool is_halted = false; - bool use_stamped_vel_ = true; bool reset(); void halt(); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 23097c0df2..ea08aef89b 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -31,7 +31,6 @@ namespace { constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; -constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; @@ -302,7 +301,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; - use_stamped_vel_ = params_.use_stamped_vel; limiter_linear_ = SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, @@ -340,50 +338,25 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( previous_commands_.emplace(empty_twist); // initialize command subscriber - if (use_stamped_vel_) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); - } - else - { - velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); - } + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); // initialize odometry publisher and messasge odometry_publisher_ = get_node()->create_publisher( @@ -534,7 +507,6 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 5d20970cab..46d89ae2d6 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -99,11 +99,6 @@ diff_drive_controller: default_value: 10, description: "Size of the rolling window for calculation of mean velocity use in odometry.", } - use_stamped_vel: { - type: bool, - default_value: true, - description: "Use stamp from input velocity message to calculate how old the command actually is.", - } publish_rate: { type: double, default_value: 50.0, # Hz From 71ec842bed2f024628bec614b4e6326488edf307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 7 Nov 2023 13:47:23 +0100 Subject: [PATCH 14/19] [CI] Re-add humble workflow files and add iron to readme (#818) --- .github/workflows/README.md | 22 ------------- .../workflows/humble-abi-compatibility.yml | 20 +++++++++++ .../workflows/humble-binary-build-main.yml | 26 +++++++++++++++ .../workflows/humble-binary-build-testing.yml | 26 +++++++++++++++ .../workflows/humble-rhel-binary-build.yml | 33 +++++++++++++++++++ .../humble-semi-binary-build-main.yml | 25 ++++++++++++++ .../humble-semi-binary-build-testing.yml | 25 ++++++++++++++ .github/workflows/humble-source-build.yml | 19 +++++++++++ README.md | 1 + ros2_controllers-not-released.iron.repos | 6 ++++ 10 files changed, 181 insertions(+), 22 deletions(-) delete mode 100644 .github/workflows/README.md create mode 100644 .github/workflows/humble-abi-compatibility.yml create mode 100644 .github/workflows/humble-binary-build-main.yml create mode 100644 .github/workflows/humble-binary-build-testing.yml create mode 100644 .github/workflows/humble-rhel-binary-build.yml create mode 100644 .github/workflows/humble-semi-binary-build-main.yml create mode 100644 .github/workflows/humble-semi-binary-build-testing.yml create mode 100644 .github/workflows/humble-source-build.yml create mode 100644 ros2_controllers-not-released.iron.repos diff --git a/.github/workflows/README.md b/.github/workflows/README.md deleted file mode 100644 index 7727495a97..0000000000 --- a/.github/workflows/README.md +++ /dev/null @@ -1,22 +0,0 @@ -## Build status - -ROS2 Distro | Branch | Build status | Documentation | Released packages -:---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) - - -### Explanation of different build types - -**NOTE**: There are three build stages checking current and future compatibility of the package. - -1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that direct local build is possible. - - Uses repos file: `$NAME$-not-released..repos` - -1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. - Shows that local build with dependencies is possible and if fails there we can expect that after the next package sync we will not be able to build. - - Uses repos file: `$NAME$.repos` - -1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml new file mode 100644 index 0000000000..708ea5c1f4 --- /dev/null +++ b/.github/workflows/humble-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Humble - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: humble + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml new file mode 100644 index 0000000000..64d78f281a --- /dev/null +++ b/.github/workflows/humble-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml new file mode 100644 index 0000000000..524cacd685 --- /dev/null +++ b/.github/workflows/humble-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml new file mode 100644 index 0000000000..9da6059892 --- /dev/null +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Humble RHEL Binary Build +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_rhel_binary: + name: Humble RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: ghcr.io/ros-controls/ros:humble-rhel + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml new file mode 100644 index 0000000000..863df79a22 --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml new file mode 100644 index 0000000000..6286636e1f --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - humble + pull_request: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml new file mode 100644 index 0000000000..ff0fd62e05 --- /dev/null +++ b/.github/workflows/humble-source-build.yml @@ -0,0 +1,19 @@ +name: Humble Source Build +on: + workflow_dispatch: + branches: + - humble + push: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + with: + ros_distro: humble + ref: humble + ros2_repo_branch: humble diff --git a/README.md b/README.md index 49d6b28ea0..d41ec09b6c 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ Commonly used and generalized controllers for ros2-control framework that are re ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build-main.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types diff --git a/ros2_controllers-not-released.iron.repos b/ros2_controllers-not-released.iron.repos new file mode 100644 index 0000000000..1b3910e7e7 --- /dev/null +++ b/ros2_controllers-not-released.iron.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master From 0196eeb29ad0e2c908b547f57d31a8a1ea13e869 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 9 Nov 2023 17:15:45 -0500 Subject: [PATCH 15/19] [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (#822) --- .../src/diff_drive_controller_parameter.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 46d89ae2d6..0c0285e7c2 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -17,7 +17,7 @@ diff_drive_controller: wheels_per_side: { type: int, default_value: 0, - description: "Number of wheels on each wide of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number or control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", } wheel_radius: { type: double, @@ -87,7 +87,7 @@ diff_drive_controller: cmd_vel_timeout: { type: double, default_value: 0.5, # seconds - description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", + description: "Timeout in seconds, after which input command on ``cmd_vel`` topic is considered staled.", } publish_limited_velocity: { type: bool, From 232154dab5f5363704c7f7640b2d42f5d6f3665b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 11 Nov 2023 15:30:19 +0100 Subject: [PATCH 16/19] [CI] Update coverage workflows and some cleanup (#819) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add CI coverage jobs for humble and iron * Add all packages to source build * Update repos files * Build ros-control packages from source * use humble job names Co-authored-by: Christoph Fröhlich --------- Co-authored-by: Bence Magyar --- .../workflows/ci-coverage-build-humble.yml | 65 +++++++++++++++++++ .github/workflows/ci-coverage-build-iron.yml | 65 +++++++++++++++++++ .github/workflows/ci-coverage-build.yml | 2 +- .../reusable-ros-tooling-source-build.yml | 19 ++++++ ros2_controllers.iron.repos | 8 --- ros2_controllers.rolling.repos | 5 -- 6 files changed, 150 insertions(+), 14 deletions(-) create mode 100644 .github/workflows/ci-coverage-build-humble.yml create mode 100644 .github/workflows/ci-coverage-build-iron.yml diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml new file mode 100644 index 0000000000..46922ccaac --- /dev/null +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -0,0 +1,65 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + pull_request: + branches: + - humble + +jobs: + coverage: + name: coverage build + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + env: + ROS_DISTRO: humble + steps: + - uses: ros-tooling/setup-ros@0.7.0 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-ci@0.3.4 + with: + target-ros2-distro: ${{ env.ROS_DISTRO }} + import-token: ${{ secrets.GITHUB_TOKEN }} + # build all packages listed here + package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + position_controllers + range_sensor_broadcaster + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers + + vcs-repo-file-url: | + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - uses: codecov/codecov-action@v3.1.4 + with: + file: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + - uses: actions/upload-artifact@v3.1.3 + 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 new file mode 100644 index 0000000000..723654b33e --- /dev/null +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -0,0 +1,65 @@ +name: Coverage Build - Iron +on: + workflow_dispatch: + push: + branches: + - iron + pull_request: + branches: + - iron + +jobs: + coverage: + name: coverage build + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + env: + ROS_DISTRO: iron + steps: + - uses: ros-tooling/setup-ros@0.7.0 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-ci@0.3.4 + with: + target-ros2-distro: ${{ env.ROS_DISTRO }} + import-token: ${{ secrets.GITHUB_TOKEN }} + # build all packages listed here + package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + position_controllers + range_sensor_broadcaster + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers + + vcs-repo-file-url: | + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - uses: codecov/codecov-action@v3.1.4 + with: + file: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + - uses: actions/upload-artifact@v3.1.3 + 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 694f3dd8a1..6a932a2dcd 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -46,7 +46,7 @@ jobs: velocity_controllers vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-defaults: | { "build": { diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index ad3dcd505d..a444a7f645 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -38,7 +38,26 @@ jobs: ref: ${{ inputs.ref }} # build all packages listed in the meta package package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + position_controllers + range_sensor_broadcaster + ros2_controllers + ros2_controllers_test_nodes + rqt_joint_trajectory_controller + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers vcs-repo-file-url: | https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos diff --git a/ros2_controllers.iron.repos b/ros2_controllers.iron.repos index 4e2c7abbbd..7f2db052cb 100644 --- a/ros2_controllers.iron.repos +++ b/ros2_controllers.iron.repos @@ -19,11 +19,3 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 42ec854e25..8c20eccc96 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -15,15 +15,10 @@ repositories: type: git url: https://github.com/ros-controls/control_toolbox.git version: ros2-master - kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git From f1a5397eb169f4358651654d144a0dea2e89ff1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Nov 2023 12:18:38 +0100 Subject: [PATCH 17/19] [JTC] Remove deprecation warnings, set `allow_nonzero_velocity_at_trajectory_end` default false (#834) --- .../doc/parameters.rst | 3 +-- .../joint_trajectory_controller.hpp | 4 ++-- .../src/joint_trajectory_controller.cpp | 24 +++---------------- ...oint_trajectory_controller_parameters.yaml | 7 +----- .../test/test_trajectory_actions.cpp | 6 ++++- .../test/test_trajectory_controller.cpp | 23 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 20 +++++++++------- 7 files changed, 34 insertions(+), 53 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 95a6599191..8ad2b406d6 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -66,7 +66,7 @@ start_with_holding (bool) allow_nonzero_velocity_at_trajectory_end (boolean) If false, the last velocity point has to be zero or the goal will be rejected. - Default: true + Default: false cmd_timeout (double) Timeout after which the input command is considered stale. @@ -147,5 +147,4 @@ gains..angle_wraparound (bool) Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured position :math:`s` from the state interface. - Default: false 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 0366c8e6d6..bf75e3f99b 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 @@ -167,8 +167,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; - // Configuration for every joint, if position error is normalized - std::vector normalize_joint_error_; + // Configuration for every joint, if position error is wrapped around + std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9e0b4823f3..88dd75b08a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -64,15 +64,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() return CallbackReturn::ERROR; } - // TODO(christophfroehlich): remove deprecation warning - if (params_.allow_nonzero_velocity_at_trajectory_end) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to " - "true. The default behavior will change to false."); - } - return CallbackReturn::SUCCESS; } @@ -131,7 +122,7 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - if (normalize_joint_error_[index]) + if (joints_angle_wraparound_[index]) { // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -pihas_effort_command_interface()) == false) + { + // can't succeed with effort cmd if + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + } } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 07d60ce077..b2efc44bff 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -201,8 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params); // send msg @@ -456,14 +455,13 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; /** - * @brief check if position error of revolute joints are normalized if not configured so + * @brief check if position error of revolute joints are angle_wraparound if not configured so */ -TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); subscribeToState(); @@ -706,14 +704,13 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) } /** - * @brief check if position error of revolute joints are normalized if configured so + * @brief check if position error of revolute joints are angle_wraparound if configured so */ -TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); subscribeToState(); @@ -754,7 +751,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) 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] normalized? + // 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( @@ -783,7 +780,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) 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] normalized? + // 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], @@ -811,7 +808,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) 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] normalized? + // 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], diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7c65fec29c..69f41d2cd9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -198,7 +198,7 @@ class TrajectoryControllerTest : public ::testing::Test } void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) + double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -211,27 +211,31 @@ class TrajectoryControllerTest : public ::testing::Test 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 normalize_error(prefix + ".normalize_error", normalize_error_default); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); + const rclcpp::Parameter angle_wraparound( + prefix + ".angle_wraparound", angle_wraparound_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool normalize_error = false) + bool angle_wraparound = false) { 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, normalize_error); + SetPidParameters(k_p, ff, angle_wraparound); + + // set optional parameters for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); } - // ignore velocity tolerances for this test since they aren't committed in test_robot->write() - rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); traj_controller_->get_node()->configure(); From b13ae5b5c35dfb40283a51b5fe0285d41571aa5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Nov 2023 19:14:09 +0100 Subject: [PATCH 18/19] [JTC] Fix tests when state offset is used (#797) * Simplify initialization of states * Rename read_state_from_hardware method * Don't set state_desired in on_activate --------- Co-authored-by: Dr. Denis --- .../joint_trajectory_controller.hpp | 8 +- .../src/joint_trajectory_controller.cpp | 19 +-- .../test/test_trajectory_controller.cpp | 118 +++++++++++------- .../test/test_trajectory_controller_utils.hpp | 72 +++++++---- 4 files changed, 135 insertions(+), 82 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 bf75e3f99b..eaf821aa26 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 @@ -261,8 +261,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_hardware(JointTrajectoryPoint & state); + void read_state_from_state_interfaces(JointTrajectoryPoint & state); + /** Assign values from the command interfaces as state. + * This is only possible if command AND state interfaces exist for the same type, + * therefore needs check for both. + * @param[out] state to be filled with values from command interfaces. + * @return true if all interfaces exists and contain non-NaN values, false otherwise. + */ bool read_state_from_command_interfaces(JointTrajectoryPoint & state); bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 88dd75b08a..cf35156541 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update( // current state update state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); + read_state_from_state_interfaces(state_current_); // currently carrying out a trajectory if (has_active_trajectory()) @@ -397,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) @@ -951,20 +951,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; - // Initialize current state storage if hardware state has tracking offset - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); if (read_state_from_command_interfaces(state)) { state_current_ = state; - state_desired_ = state; last_commanded_state_ = state; } + else + { + // Initialize current state storage from hardware + read_state_from_state_interfaces(state_current_); + 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) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index b2efc44bff..117575be37 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1668,21 +1668,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e #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 state interfaces +// time and hardware state has offset --> this is indicated by NaN values in command interfaces TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = std::numeric_limits::quiet_NaN(); - joint_vel_[i] = std::numeric_limits::quiet_NaN(); - joint_acc_[i] = std::numeric_limits::quiet_NaN(); - } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + std::vector initial_pos_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_vel_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; + + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from state interfaces + // (command interface are NaN) auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1691,70 +1693,96 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); } // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); } } executor.cancel(); } -// Testing that values are read from state interfaces when hardware is started after some values +// Testing that values are read from command interfaces when hardware is started after some values // are set on the hardware commands TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - // set command values to NaN + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; for (size_t i = 0; i < 3; ++i) { - joint_pos_[i] = 3.1 + static_cast(i); - joint_vel_[i] = 0.25 + static_cast(i); - joint_acc_[i] = 0.02 + static_cast(i) / 10.0; + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); for (size_t i = 0; i < 3; ++i) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - - // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + // check position + if (traj_controller_->has_position_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + if (traj_controller_->has_velocity_command_interface()) + { + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + if (traj_controller_->has_acceleration_command_interface()) + { + // should have set it to last position + velocity + acceleration command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + else + { + // should have set it to last position + velocity command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + } + else + { + // should have set it to last position command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + } } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + else { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 69f41d2cd9..7118882da1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -220,7 +220,11 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false) + bool angle_wraparound = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { SetUpTrajectoryController(executor); @@ -239,12 +243,17 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->get_node()->configure(); - ActivateTrajectoryController(separate_cmd_and_state_values); + ActivateTrajectoryController( + separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, + initial_eff_joints); } void ActivateTrajectoryController( bool separate_cmd_and_state_values = false, - const std::vector initial_pos_joints = INITIAL_POS_JOINTS) + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -280,14 +289,17 @@ class TrajectoryControllerTest : public ::testing::Test cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); + cmd_interfaces.back().set_value(initial_vel_joints[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); + cmd_interfaces.back().set_value(initial_acc_joints[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); - joint_state_pos_[i] = initial_pos_joints[i]; - joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; - joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + cmd_interfaces.back().set_value(initial_eff_joints[i]); + if (separate_cmd_and_state_values) + { + joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; + joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + } state_interfaces.emplace_back(pos_state_interfaces_.back()); state_interfaces.emplace_back(vel_state_interfaces_.back()); state_interfaces.emplace_back(acc_state_interfaces_.back()); @@ -489,27 +501,33 @@ class TrajectoryControllerTest : public ::testing::Test // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is " - << joint_vel_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is " - << joint_vel_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is " - << joint_vel_[2]; + 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]; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is " - << joint_eff_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is " - << joint_eff_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is " - << joint_eff_[2]; + 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]; } } } From 3fc0f50c440ad49b2194b2c7dae8c54d6cda7b07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Nov 2023 20:03:40 +0100 Subject: [PATCH 19/19] [JTC] Activate update of dynamic parameters (#761) --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 95 +++++++++++++------ .../test/test_trajectory_controller.cpp | 36 +++++++ .../test/test_trajectory_controller_utils.hpp | 2 + 4 files changed, 104 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 eaf821aa26..7542aa6951 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 @@ -277,6 +277,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr response); private: + void update_pids(); + bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cf35156541..3936da155f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -115,6 +115,17 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + // 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_); + } + } auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, size_t index, @@ -704,15 +715,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } + update_pids(); } // Configure joint position error normalization from ROS parameters (angle_wraparound) @@ -787,8 +790,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.command_interfaces).c_str(), get_interface_list(params_.state_interfaces).c_str()); - default_tolerances_ = get_segment_tolerances(params_); - + // parse remaining parameters const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -874,32 +876,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); - if (params_.cmd_timeout > 0.0) - { - if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) - { - cmd_timeout_ = params_.cmd_timeout; - } - else - { - // deactivate timeout - RCLCPP_WARN( - logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", - params_.cmd_timeout, default_tolerances_.goal_time_tolerance); - cmd_timeout_ = 0.0; - } - } - else - { - cmd_timeout_ = 0.0; - } - return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -974,6 +965,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } rt_is_holding_.writeFromNonRT(true); + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } @@ -1530,6 +1543,26 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } +} + void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_ = std::make_shared(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 117575be37..7842d50434 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -413,6 +413,42 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateController(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } + + 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 7118882da1..224265ad83 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -130,6 +130,8 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + std::vector get_pids() const { return pids_; } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const