From 0a06a2d7cbeaeb08aacfc88bebd4e258a877489c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 8 Jul 2021 20:12:35 +0100 Subject: [PATCH 001/524] Fix test dependencies (#213) --- effort_controllers/package.xml | 2 +- gripper_controllers/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 4e8e60cc9c..a2ef6ceee3 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -15,7 +15,7 @@ pluginlib - ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common controller_manager diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 6fef796f30..6ff10c4f92 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -24,7 +24,7 @@ pluginlib - ament_cmake_gtest + ament_cmake_gmock ament_lint_auto ament_lint_common controller_manager From 1c7d38a7d544b4f58fc2171d87dba93b2ffe046c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 8 Jul 2021 20:13:15 +0100 Subject: [PATCH 002/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_state_controller/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 12 files changed, 40 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1efc32a10f..4b0ce3eadb 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b2ce4501d5..0600579a56 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix test dependencies (`#213 `_) +* Contributors: Bence Magyar + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 88778837bb..28529229e3 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 +----------- + 0.4.0 (2021-06-28) ------------------ * Fix dependency (`#208 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index bcc8d57fc2..2d7620a0df 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4901c3427f..5dd6333ef4 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix test dependencies (`#213 `_) +* Contributors: Bence Magyar + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 534cc1a26e..17fac0715e 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Add imu sensor broadcaster (`#195 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index b605a574d5..ebc98c3dcd 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst index 991d4f6e24..8482f5b891 100644 --- a/joint_state_controller/CHANGELOG.rst +++ b/joint_state_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 97f165d789..dbe3c7585e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 55ef8d9519..7bd8d21009 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8d695925b6..221c8bb281 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Add imu sensor broadcaster (`#195 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bf67f215fd..b2c367344d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.4.0 (2021-06-28) ------------------ * Force torque sensor broadcaster (`#152 `_) From 6a53981982eeac1822bfca8e9d5e40156870c0b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 8 Jul 2021 20:13:34 +0100 Subject: [PATCH 003/524] 0.4.1 --- 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_state_controller/CHANGELOG.rst | 4 ++-- joint_state_controller/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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4b0ce3eadb..5538bc0db2 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index ffb1cb7309..0b5d6eeb6d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 0.4.0 + 0.4.1 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0600579a56..5569fbb1e6 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ * Fix test dependencies (`#213 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index a2ef6ceee3..f5f18c7dbb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 0.4.0 + 0.4.1 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 28529229e3..497ba92601 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 ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 272aa74999..901eb559cb 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 0.4.0 + 0.4.1 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 2d7620a0df..eb1fc95a17 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index b9d06b0a1b..304aee85ac 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 0.4.0 + 0.4.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5dd6333ef4..e554f53410 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ * Fix test dependencies (`#213 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 6ff10c4f92..4ade7a9d6d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 0.4.0 + 0.4.1 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 17fac0715e..66cf200abd 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index cf5315ceb6..aeb3efaa4c 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 0.4.0 + 0.4.1 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 ebc98c3dcd..ba970b1a76 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index ecf53d3820..ed6da47519 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 0.4.0 + 0.4.1 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst index 8482f5b891..2c0f757440 100644 --- a/joint_state_controller/CHANGELOG.rst +++ b/joint_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml index c23e02ca67..765eae7e79 100644 --- a/joint_state_controller/package.xml +++ b/joint_state_controller/package.xml @@ -1,7 +1,7 @@ joint_state_controller - 0.4.0 + 0.4.1 Controller to publish joint state Bence Magyar Jordan Palacios diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index dbe3c7585e..97ff2f79a1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fd85c7d3d2..a7750242c0 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 0.4.0 + 0.4.1 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 7bd8d21009..2d8acf3e09 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 66f92b544b..8d69542495 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 0.4.0 + 0.4.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 221c8bb281..b0812debf5 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 82d11e28bc..9f15523277 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 0.4.0 + 0.4.1 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b2c367344d..ac1066d64a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.4.1 (2021-07-08) +------------------ 0.4.0 (2021-06-28) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 0096d7268e..dab0f19200 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 0.4.0 + 0.4.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 12ba94f1ec592224fe56c791a042362e4b018443 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 17 Jul 2021 08:30:25 +0200 Subject: [PATCH 004/524] Reduce docs warnings and correct adding guidelines (#219) * Correct docs * More corrections --- diff_drive_controller/doc/userdoc.rst | 8 ++++---- doc/controllers_index.rst | 4 +++- doc/writing_new_controller.rst | 1 + effort_controllers/doc/userdoc.rst | 4 ++-- forward_command_controller/doc/userdoc.rst | 4 ++-- joint_state_broadcaster/doc/userdoc.rst | 11 ++++++----- joint_trajectory_controller/doc/userdoc.rst | 3 ++- position_controllers/doc/userdoc.rst | 4 ++-- velocity_controllers/doc/userdoc.rst | 4 ++-- 9 files changed, 24 insertions(+), 19 deletions(-) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 1aa5f3deba..fee26db2cd 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -1,24 +1,24 @@ .. _diff_drive_controller_userdoc: diff_drive_controller ---------------------- +===================== Controller for mobile robots with differential drive. Input for control are robot body velocity commands which are translated to wheel commands for the differential drive base. Odometry is computed from hardware feedback and published. Velocity commands -^^^^^^^^^^^^^^^^^ +----------------- The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- The controller works with wheel joints through a velocity interface. Other features -^^^^^^^^^^^^^^ +-------------- Realtime-safe implementation. Odometry publishing diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2dfaa8f6f0..f1bb64ce40 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -33,11 +33,12 @@ Guidelines and Best Practices ============================= .. toctree:: - :titlesonly: 1 + :titlesonly: :glob: * + Available Controllers ===================== @@ -51,6 +52,7 @@ Available Controllers Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> + Available Broadcasters ====================== diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 32ab800fc6..9de3a7aed3 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -104,6 +104,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Export for pluginlib description file using the following command: .. code:: cmake + pluginlib_export_plugin_description_file(controller_interface .xml) 6. Add install directives for targets and include directory. diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 0db2dd3a8d..000bfcf26c 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -1,11 +1,11 @@ .. _effort_controllers_userdoc: effort_controllers ---------------------- +================== This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- These controllers work with joints using the "effort" command interface. diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index e267905775..ce653fe714 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -1,11 +1,11 @@ .. _forward_command_controller_userdoc: forward_command_controller --------------------------- +========================== This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- These controllers work with joints using the "effort" command interface. diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 916cd03ec5..38c218266c 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -1,21 +1,22 @@ .. _joint_state_broadcaster_userdoc: joint_state_broadcaster ----------------------- +======================= The broadcaster reads all state interfaces and reports them on ``/joint_states`` and ``/dynamic_joint_states``. Commands -^^^^^^^^ +-------- Broadcasters are not real controllers, and therefore take no commands. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- All available *joint state interfaces* are used by this broadcaster. Parameters -^^^^^^^^^^ +---------- -The parameter ``extra_joints`` is optional and is an array of strings consisting of the names of the extra joints that will be added to ``/joint_states`` and ``/dynamic_joint_states`` with state set to 0. +extra_joints (optional; string array; default: empty) + Names of extra joints to be added to ``/joint_states`` and ``/dynamic_joint_states`` with state set to 0. diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 286f99c2f0..d229f60cd4 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -1,7 +1,7 @@ .. _joint_trajectory_controller_userdoc: joint_trajectory_controller ---------------------------- +=========================== Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. @@ -95,6 +95,7 @@ The following version of the Joint Trajectory Controller are available mapping t - position_velocity_acceleration_controllers::JointTrajectoryController - Input: position, [velocity, [acceleration]] - Output: position, velocity and acceleration + .. - velocity_controllers::JointTrajectoryController .. - Input: position, [velocity, [acceleration]] .. - Output: velocity diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index 3f7c27159a..1e5dd53e63 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -1,11 +1,11 @@ .. _position_controllers_userdoc: position_controllers ---------------------- +===================== This is a collection of controllers that work using the "position" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the position on a certain joint to achieve a set velocity. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- These controllers work with joints using the "position" command interface. diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index 721f946c99..62b6aa019f 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -1,11 +1,11 @@ .. _velocity_controllers_userdoc: velocity_controllers ---------------------- +==================== This is a collection of controllers that work using the "velocity" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the velocity on a certain joint to achieve a set position. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- These controllers work with joints using the "velocity" command interface. From 856e8e226a8d1a4a11ccc9a9dd5afd82e4d445e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 23 Jul 2021 22:33:58 +0200 Subject: [PATCH 005/524] Enable JTC for hardware having offset from state measurements (#189) * Avoid "jumps" with states that have tracking error. All test are passing but separatelly. Is there some kind of timeout? * Remove allow_integration_flag * Add reading from command interfaces when restarting controller * Rename variable according to review * Use even more clear name * Remove debug lines. * Update documentation * Apply suggestions from code review Co-authored-by: Bence Magyar * Move methods to cpp file as per reviewers comment. Co-authored-by: Bence Magyar --- doc/controllers_index.rst | 12 +- joint_trajectory_controller/CMakeLists.txt | 2 +- joint_trajectory_controller/doc/userdoc.rst | 100 +++++- .../joint_trajectory_controller.hpp | 22 +- .../tolerances.hpp | 4 +- .../src/joint_trajectory_controller.cpp | 171 +++++++--- .../test/test_trajectory_controller.cpp | 316 ++++++++++++++++-- .../test/test_trajectory_controller_utils.hpp | 30 +- 8 files changed, 567 insertions(+), 90 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index f1bb64ce40..63eee5b32e 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -1,13 +1,15 @@ .. _controllers: -======================== +################# ros2_controllers -======================== +################# `GitHub Repository `_ + Nomenclature -------------- +************ + The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using. The controllers are using `common hardware interface definitions`_. The controllers' namespaces are commanding the following command interface types: @@ -19,7 +21,7 @@ The controllers' namespaces are commanding the following command interface types Controllers --------------- +*********** The following standard controllers are implemented: @@ -30,7 +32,7 @@ The following standard controllers are implemented: Guidelines and Best Practices -============================= +***************************** .. toctree:: :titlesonly: diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 395ced8b0f..602b4b2a45 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -70,7 +70,7 @@ if(BUILD_TESTING) ament_add_gtest(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) - set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 120) + set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 180) target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller joint_trajectory_controller diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index d229f60cd4..a4005a5583 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -6,7 +6,7 @@ joint_trajectory_controller Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. Trajectory representation -^^^^^^^^^^^^^^^^^^^^^^^^^ +------------------------- The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: @@ -17,14 +17,14 @@ The controller is templated to work with multiple trajectory representations. By Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. Hardware interface type -^^^^^^^^^^^^^^^^^^^^^^^ +----------------------- The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here. Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). Other features -^^^^^^^^^^^^^^ +-------------- Realtime-safe implementation. @@ -34,7 +34,7 @@ Other features Using Joint Trajectory Controller(s) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +------------------------------------ The controller expects at least position feedback from the hardware. Joint velocities and accelerations are optional. @@ -54,12 +54,12 @@ A yaml file for using it could be: joint_trajectory_controller: ros__parameters: joints: - - joint0 - joint1 - joint2 - joint3 - joint4 - joint5 + - joint6 command_interfaces: - position @@ -68,19 +68,93 @@ A yaml file for using it could be: - position - velocity - state_publish_rate: 50.0 # Defaults to 50 - action_monitor_rate: 20.0 # Defaults to 20 + state_publish_rate: 50.0 + action_monitor_rate: 20.0 - allow_partial_joints_goal: false # Defaults to false - hardware_state_has_offset: true - deduce_states_from_derivatives: true + allow_partial_joints_goal: false + open_loop_control: true constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediately) + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + joint1: + trajectory: 0.05 + goal: 0.03 + + +Details about parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +joint (list(string)): + Joint names to control. + +command_interface (list(string)): + Command interfaces provided by the hardware interface for all joints. + + Values: [position | velocity | acceleration] (multiple allowed) + +state_interfaces (list(string)): + State interfaces provided by the hardware for all joints. + + Values: position (mandatory) [velocity, [acceleration]]. + Acceleration interface can only be used in combination with position and velocity. + + Default: 50.0 + +state_publish_rate (double): + Publish-rate of the controller's "state" topic. + + Default: 20.0 + +action_monitor_rate (double): + Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + +allow_partial_joints_goal (boolean): + Allow joint goals defining trajectory for only some joints. + +open_loop_control (boolean): + Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + + If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. + +constraints (structure) + Default values for tolerances if no explicit values are states in JointTrajectory message. + +constraints.stopped_velocity_tolerance (double) + Default value for end velocity deviation. + + Default: 0.01 + +constraints.goal_time (double) + Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + + Default: 0.0 (not checked) + +constraints..trajectory + Maximally allowed deviation from the target trajectory for a given joint. + + Default: 0.0 (tolerance is not enforced) + +constraints..goal + Maximally allowed deviation from the goal (end of the trajectory) for a given joint. + + Default: 0.0 (tolerance is not enforced) + + +ROS2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] + Topic for commanding the controller. + +~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] + Topic publishing internal states. + +~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] + Action server for commanding the controller. Specialized versions of JointTrajectoryController (TBD in ...) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +-------------------------------------------------------------- The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). 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 96c432c9f6..e09efc5026 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 @@ -126,6 +126,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa hardware_interface::HW_IF_EFFORT, }; + // Parameters for some special cases, e.g. hydraulics powered robots + /// Run he controller in open-loop, i.e., read hardware states only when starting controller. + /// This is useful when robot is not exactly following the commanded trajectory. + bool open_loop_control_ = false; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ @@ -142,6 +148,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_acceleration_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used + // TODO(anyone): This flag is not used for now + // There should be PID-approach used as in ROS1: + // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 bool use_closed_loop_pid_adapter = false; // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported @@ -231,13 +240,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); + void read_state_from_hardware(JointTrajectoryPoint & state); + + bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + private: bool contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type) - { - return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != - interface_type_list.end(); - } + const std::vector & interface_type_list, const std::string & interface_type); + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 86eced93eb..dbba7ca57a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -77,8 +77,8 @@ struct SegmentTolerances /** * \brief Populate trajectory segment tolerances using data from the ROS node. * - * It is assumed that the following parameter structure is followed on the provided LifecycleNode. Unspecified parameters - * will take the defaults shown in the comments: + * It is assumed that the following parameter structure is followed on the provided LifecycleNode. + * Unspecified parameters will take the defaults shown in the comments: * * \code * constraints: diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 05eaa227b9..787ab72587 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -67,6 +67,7 @@ JointTrajectoryController::init(const std::string & controller_name) node_->declare_parameter("state_publish_rate", 50.0); node_->declare_parameter("action_monitor_rate", 20.0); node_->declare_parameter("allow_partial_joints_goal", allow_partial_joints_goal_); + node_->declare_parameter("open_loop_control", open_loop_control_); node_->declare_parameter("constraints.stopped_velocity_tolerance", 0.01); node_->declare_parameter("constraints.goal_time", 0.0); @@ -108,17 +109,6 @@ JointTrajectoryController::update() return controller_interface::return_type::OK; } - auto resize_joint_trajectory_point = - [&](trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) - { - point.positions.resize(size); - if (has_velocity_state_interface_) { - point.velocities.resize(size); - } - if (has_acceleration_state_interface_) { - point.accelerations.resize(size); - } - }; auto compute_error_for_joint = [&](JointTrajectoryPoint & error, int index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) { @@ -146,13 +136,6 @@ JointTrajectoryController::update() const auto joint_num = joint_names_.size(); resize_joint_trajectory_point(state_current, joint_num); - auto assign_point_from_interface = [&, joint_num]( - std::vector & trajectory_point_interface, const auto & joint_inteface) - { - for (auto index = 0ul; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); - } - }; // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = [&, joint_num]( @@ -165,32 +148,19 @@ JointTrajectoryController::update() // current state update state_current.time_from_start.set__sec(0); - - // Assign values from the hardware - // Position states always exist - assign_point_from_interface(state_current.positions, joint_state_interface_[0]); - // velocity and acceleration states are optional - if (has_velocity_state_interface_) { - assign_point_from_interface(state_current.velocities, joint_state_interface_[1]); - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) { - assign_point_from_interface(state_current.accelerations, joint_state_interface_[2]); - } else { - // Make empty so the property is ignored during interpolation - state_current.accelerations.clear(); - } - } else { - // Make empty so the property is ignored during interpolation - state_current.velocities.clear(); - state_current.accelerations.clear(); - } + read_state_from_hardware(state_current); // currently carrying out a trajectory if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { // if sampling the first time, set the point before you sample if (!(*traj_point_active_ptr_)->is_sampled_already()) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg( - node_->now(), state_current); + if (open_loop_control_) { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg( + node_->now(), last_commanded_state_); + } else { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg( + node_->now(), state_current); + } } resize_joint_trajectory_point(state_error, joint_num); @@ -240,6 +210,9 @@ JointTrajectoryController::update() } } + // store command as state when hardware state has tracking offset + last_commanded_state_ = state_desired; + const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { // send feedback @@ -307,6 +280,92 @@ JointTrajectoryController::update() return controller_interface::return_type::OK; } +void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +{ + const auto joint_num = joint_names_.size(); + auto assign_point_from_interface = [&, joint_num]( + std::vector & trajectory_point_interface, const auto & joint_inteface) + { + for (auto index = 0ul; index < joint_num; ++index) { + trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + } + }; + + // Assign values from the hardware + // Position states always exist + assign_point_from_interface(state.positions, joint_state_interface_[0]); + // velocity and acceleration states are optional + if (has_velocity_state_interface_) { + assign_point_from_interface(state.velocities, joint_state_interface_[1]); + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) { + assign_point_from_interface(state.accelerations, joint_state_interface_[2]); + } else { + // Make empty so the property is ignored during interpolation + state.accelerations.clear(); + } + } else { + // Make empty so the property is ignored during interpolation + state.velocities.clear(); + state.accelerations.clear(); + } +} + +bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) +{ + bool has_values = true; + + const auto joint_num = joint_names_.size(); + auto assign_point_from_interface = [&, joint_num]( + std::vector & trajectory_point_interface, const auto & joint_inteface) + { + for (auto index = 0ul; index < joint_num; ++index) { + trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + } + }; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) {return std::isnan(interface.get().get_value());}) == + joint_interface.end(); + }; + + // Assign values from the command interfaces as state. Therefore needs check for both. + // Position state interface has to exist always + if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) { + assign_point_from_interface(state.positions, joint_command_interface_[0]); + } else { + state.positions.clear(); + has_values = false; + } + // velocity and acceleration states are optional + if (has_velocity_state_interface_) { + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) { + assign_point_from_interface(state.velocities, joint_command_interface_[1]); + } else { + state.velocities.clear(); + has_values = false; + } + } else { + state.velocities.clear(); + } + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) { + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) { + assign_point_from_interface(state.accelerations, joint_command_interface_[2]); + } else { + state.accelerations.clear(); + has_values = false; + } + } else { + state.accelerations.clear(); + } + + return has_values; +} + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) { @@ -466,6 +525,10 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) default_tolerances_ = get_segment_tolerances(*node_, joint_names_); + // Read parameters customizing controller for special cases + open_loop_control_ = + node_->get_parameter("open_loop_control").get_value(); + // subscriber callback // non realtime // TODO(karsten): check if traj msg and point time are valid @@ -630,6 +693,17 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) traj_point_active_ptr_ = &traj_external_point_ptr_; last_state_publish_time_ = node_->now(); + // Initialize current state storage if hardware state has tracking offset + resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading last_commanded_state_ from commands is + // those are not nan + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, joint_names_.size()); + if (read_state_from_command_interfaces(state)) { + last_commanded_state_ = state; + } + // TODO(karsten1987): activate subscriptions of subscriber return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -1006,6 +1080,25 @@ void JointTrajectoryController::set_hold_position() add_new_trajectory_msg(traj_msg); } +bool JointTrajectoryController::contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type) +{ + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); +} + +void JointTrajectoryController::resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + point.positions.resize(size); + if (has_velocity_state_interface_) { + point.velocities.resize(size); + } + if (has_acceleration_state_interface_) { + point.accelerations.resize(size); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6bc284ad84..0816dff5e6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -85,15 +86,16 @@ TEST_P(TrajectoryControllerTestParameterized, configure) traj_controller_->update(); - // no change in hw position - EXPECT_NE(3.3, joint_pos_[0]); - EXPECT_NE(4.4, joint_pos_[1]); - EXPECT_NE(5.5, joint_pos_[2]); + // hw position == 0 because controller is not activated + EXPECT_EQ(0.0, joint_pos_[0]); + EXPECT_EQ(0.0, joint_pos_[1]); + EXPECT_EQ(0.0, joint_pos_[2]); executor.cancel(); } -TEST_P(TrajectoryControllerTestParameterized, activate) { +TEST_P(TrajectoryControllerTestParameterized, activate) +{ SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -231,7 +233,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) { // executor.cancel(); // } -TEST_P(TrajectoryControllerTestParameterized, cleanup) { +TEST_P(TrajectoryControllerTestParameterized, cleanup) +{ SetUpAndActivateTrajectoryController(); auto traj_node = traj_controller_->get_node(); @@ -265,7 +268,8 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) { executor.cancel(); } -TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { +TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) +{ SetUpTrajectoryController(false); // This call is replacing the way parameters are set via launch @@ -336,7 +340,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } -TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +{ rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {}, &executor); subscribeToState(); @@ -421,18 +426,21 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou /** * @brief test_state_publish_rate Test that state publish rate matches configure rate */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) { +TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +{ test_state_publish_rate_target(10); } -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) { +TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +{ test_state_publish_rate_target(0); } /** * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from internal controller order */ -TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) { +TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +{ rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {}, &executor); { @@ -467,7 +475,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) { /** * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +{ rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; @@ -523,7 +532,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { /** * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints without allow_partial_joints_goal */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) { +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +{ rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); rclcpp::executors::SingleThreadedExecutor executor; @@ -599,7 +609,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe /** * @brief invalid_message Test mismatched joint and reference vector sizes */ -TEST_P(TrajectoryControllerTestParameterized, invalid_message) { +TEST_P(TrajectoryControllerTestParameterized, invalid_message) +{ rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); @@ -661,7 +672,8 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) { /** * @brief test_trajectory_replace Test replacing an existing trajectory */ -TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) { +TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +{ rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); @@ -679,9 +691,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) { expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; // Check that we reached end of points_old trajectory // Denis: delta was 0.1 with 0.2 works for me - std::cout << "Now waiting for state" << std::endl; waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - std::cout << "After waiting for state" << std::endl; RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory"); publish(time_from_start, points_partial_new); @@ -696,7 +706,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) { /** * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory */ -TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { +TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +{ rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {}, &executor); subscribeToState(); @@ -750,7 +761,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory } -TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) { +TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +{ SetUpTrajectoryController(); auto traj_node = traj_controller_->get_node(); RCLCPP_WARN( @@ -759,6 +771,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 return; + // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(traj_node->get_node_base_interface()); subscribeToState(); @@ -795,6 +808,271 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur delay * (2 + 2)), 0.1); } +TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set paramter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); + SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +TEST_P( + TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set paramter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(); + // There should not be backward commands + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(); + // There should not be a jump toward commands + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + executor.cancel(); +} + +// 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 +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 paramter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + for (auto i = 0u; 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(true, {is_open_loop_parameters}, &executor, true); + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (auto i = 0u; i < 3; ++i) { + 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() && + std::find( + command_interface_types_.begin(), command_interface_types_.end(), + hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + + // check acceleration + if (std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && + std::find( + command_interface_types_.begin(), command_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + } + + executor.cancel(); +} + +// Testing that values are read from state 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 paramter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + + // set command values to NaN + for (auto i = 0u; i < 3; ++i) { + joint_pos_[i] = 3.1 + i; + joint_vel_[i] = 0.25 + i; + joint_acc_[i] = 0.02 + i / 10.0; + } + SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + + auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + + for (auto i = 0u; 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() && + std::find( + command_interface_types_.begin(), command_interface_types_.end(), + hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + } + + // check acceleration + if (std::find( + state_interface_types_.begin(), state_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && + std::find( + command_interface_types_.begin(), command_interface_types_.end(), + hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + } + } + + executor.cancel(); +} + // TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 89b8699be2..0680a53c95 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -92,6 +92,11 @@ class TestableJointTrajectoryController : public joint_trajectory_controller:: state_interface_types_ = state_interfaces; } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() + { + return last_commanded_state_; + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -109,8 +114,11 @@ class TrajectoryControllerTest : public ::testing::Test joint_names_ = {"joint1", "joint2", "joint3"}; joint_pos_.resize(joint_names_.size(), 0.0); + joint_state_pos_.resize(joint_names_.size(), 0.0); joint_vel_.resize(joint_names_.size(), 0.0); + joint_state_vel_.resize(joint_names_.size(), 0.0); joint_acc_.resize(joint_names_.size(), 0.0); + joint_state_acc_.resize(joint_names_.size(), 0.0); // Default interface values - they will be overwritten by parameterized tests command_interface_types_ = {"position"}; state_interface_types_ = {"position", "velocity"}; @@ -146,7 +154,8 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( bool use_local_parameters = true, const std::vector & parameters = {}, - rclcpp::Executor * executor = nullptr) + rclcpp::Executor * executor = nullptr, + bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); @@ -163,10 +172,10 @@ class TrajectoryControllerTest : public ::testing::Test traj_node_->set_parameter(stopped_velocity_parameters); traj_controller_->configure(); - ActivateTrajectoryController(); + ActivateTrajectoryController(separate_cmd_and_state_values); } - void ActivateTrajectoryController() + void ActivateTrajectoryController(bool separate_cmd_and_state_values = false) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -193,15 +202,18 @@ class TrajectoryControllerTest : public ::testing::Test pos_state_interfaces_.emplace_back( hardware_interface::StateInterface( joint_names_[i], - hardware_interface::HW_IF_POSITION, &joint_pos_[i])); + hardware_interface::HW_IF_POSITION, + separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); vel_state_interfaces_.emplace_back( hardware_interface::StateInterface( joint_names_[i], - hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); + hardware_interface::HW_IF_VELOCITY, + separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); acc_state_interfaces_.emplace_back( hardware_interface::StateInterface( joint_names_[i], - hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + hardware_interface::HW_IF_ACCELERATION, + separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); @@ -210,6 +222,9 @@ class TrajectoryControllerTest : public ::testing::Test 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]); + 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()); @@ -364,6 +379,9 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_pos_; std::vector joint_vel_; std::vector joint_acc_; + std::vector joint_state_pos_; + std::vector joint_state_vel_; + std::vector joint_state_acc_; std::vector pos_cmd_interfaces_; std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; From a86a8c24b9543035f9db3458d4de51a37c6ed5d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 28 Jul 2021 19:44:56 +0200 Subject: [PATCH 006/524] Add initial pre-commit setup. (#220) * Add initial pre-commit setup. * ignore changelog files for codespell * Basic whitespace and typo fixes * Basic whitespace and typo fixes * fix docs Co-authored-by: Bence Magyar --- .github/workflows/format.yml | 24 ++++ .github/workflows/prerelease.yaml | 1 - .pre-commit-config.yaml | 132 ++++++++++++++++++ README.md | 14 +- .../src/diff_drive_controller.cpp | 2 +- doc/writing_new_controller.rst | 14 +- effort_controllers/CHANGELOG.rst | 1 - .../test_joint_group_effort_controller.cpp | 2 +- .../test_joint_group_effort_controller.hpp | 2 +- .../test_force_torque_sensor_broadcaster.hpp | 2 +- .../test/test_forward_command_controller.cpp | 4 +- .../test/test_forward_command_controller.hpp | 2 +- .../test/test_imu_sensor_broadcaster.hpp | 2 +- .../test/test_joint_state_broadcaster.hpp | 2 +- joint_trajectory_controller/doc/userdoc.rst | 2 - .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- .../test_joint_trajectory_controller.yaml | 2 - .../test/test_trajectory_actions.cpp | 2 +- .../test/test_trajectory_controller.cpp | 10 +- .../test/test_trajectory_controller_utils.hpp | 2 +- position_controllers/CHANGELOG.rst | 1 - .../test_joint_group_position_controller.cpp | 2 +- .../test_joint_group_position_controller.hpp | 2 +- ros2_controllers/CHANGELOG.rst | 1 - velocity_controllers/CHANGELOG.rst | 1 - .../test_joint_group_velocity_controller.cpp | 2 +- .../test_joint_group_velocity_controller.hpp | 2 +- 28 files changed, 192 insertions(+), 45 deletions(-) create mode 100644 .github/workflows/format.yml create mode 100644 .pre-commit-config.yaml diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000000..4eec522b51 --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,24 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + push: + branches: + - master + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + - name: Install clang-format-10 + run: sudo apt-get install clang-format-10 cppcheck + - uses: pre-commit/action@v2.0.3 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index 1ec05467fd..8b860855cf 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -24,4 +24,3 @@ jobs: - uses: actions/checkout@v2 - name: industrial_ci uses: ros-industrial/industrial_ci@master - diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000000..16acfea157 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,132 @@ + +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + - id: fix-byte-order-marker + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v2.12.0 + hooks: + - id: pyupgrade + args: [--py36-plus] + + # PEP 257 + - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 + rev: v0.3.3 + hooks: + - id: pep257 + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + + - repo: https://github.com/pycqa/flake8 + rev: 3.9.0 + hooks: + - id: flake8 + args: ["--ignore=E501"] + + # CPP hooks + - repo: local + hooks: + - id: ament_uncrustify + name: ament_uncrustify + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-whitespace/newline"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + # Copyright + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: 0.9.0a1 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.8.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.0.0 + hooks: + - id: codespell + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc)$ diff --git a/README.md b/README.md index 3046fda83d..ce0650020d 100644 --- a/README.md +++ b/README.md @@ -8,21 +8,21 @@ Have a look at here: https://github.com/ros-controls/ros2_control ## Acknowledgements - - rosin_logo -Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. +Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. More information: rosin-project.eu -eu_flag +eu_flag -This project has received funding from the European Union’s Horizon 2020 -research and innovation programme under grant agreement no. 732287. \ No newline at end of file +This project has received funding from the European Union’s Horizon 2020 +research and innovation programme under grant agreement no. 732287. diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e09e2cc08c..a4ddbbbc6a 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -498,7 +498,7 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( node_->get_logger(), - "Either left wheel interfaces, right wheel interfaces are non existant"); + "Either left wheel interfaces, right wheel interfaces are non existent"); return CallbackReturn::ERROR; } diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 9de3a7aed3..cf589b86f9 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -11,7 +11,7 @@ The following is a step-by-step guide to create source files, basic tests, and c If the package for the controller does not exist, then create it first. The package should have ``ament_cmake`` as a build type. The easiest way is to search online for the most recent manual. - A helpful command to support this process is `ros2 pkg create`. + A helpful command to support this process is ``ros2 pkg create``. Use the ``--help`` flag for more information on how to use it. There is also an option to create library source files and compile rules to help you in the following steps. @@ -33,10 +33,10 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Define the class of the controller, extending ``ControllerInterface``, e.g., .. code:: c++ - class ControllerName : public controller_interface::ControllerInterface + class ControllerName : public controller_interface::ControllerInterface 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. - For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. + For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. @@ -76,7 +76,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. **Writing export definition for pluginlib** 1. Create the ``.xml`` file in the package and add a definition of the library and controller's class which has to be visible for the pluginlib. - The easiest way to do that is to check other controllers in the `ros2_controllers `_ package. + The easiest way to do that is to check other controllers in the `ros2_controllers `_ package. 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. @@ -85,9 +85,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 6. **Writing simple test to check if the controller can be found and loaded** - 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp>. + 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can safely copy the file's content for any controller defined in the `ros2_controllers `_ package. + 2. You can safely copy the file's content for any controller defined in the `ros2_controllers `_ package. 3. Change the name of the copied test and in the last line, where controller type is specified put the name defined in ``.xml`` file, e.g., ``/``. @@ -112,7 +112,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``controller_manager``, ``hardware_interface``, ``ros2_control_test_assets``. 8. Add compile definitions for the tests using the ``ament_add_gmock`` directive. - For details, see how it is done for controllers in the `ros2_controllers `_ package. + For details, see how it is done for controllers in the `ros2_controllers `_ package. 9. (optional) Add your controller`s library into ``ament_export_libraries`` before ``ament_package()``. diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 5569fbb1e6..6baa8894d5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -43,4 +43,3 @@ Changelog for package effort_controllers 0.1.0 (2020-12-23) ------------------ - diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 9922009927..b27367bfd8 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -151,7 +151,7 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong numnber of joints + // send command with wrong number of joints auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index bb058b0282..96a6c83ea6 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -27,7 +27,7 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::CommandInterface; -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController { FRIEND_TEST(JointGroupEffortControllerTest, CommandSuccessTest); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index f4ccfe408d..8104b7e85c 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -26,7 +26,7 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendForceTorqueSensorBroadcaster : public force_torque_sensor_broadcaster:: ForceTorqueSensorBroadcaster { diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 7893064977..cbc28f67fb 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -115,7 +115,7 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) { SetUpController(); - // configure failed, 'interface_name' paremeter not set + // configure failed, 'interface_name' parameter not set controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); controller_->get_node()->set_parameter({"interface_name", ""}); @@ -219,7 +219,7 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong numnber of joints + // send command with wrong number of joints auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 2532296414..5b816bca52 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -28,7 +28,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::CommandInterface; -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendForwardCommandController : public forward_command_controller::ForwardCommandController { FRIEND_TEST(ForwardCommandControllerTest, JointsParameterNotSet); diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 5773bfeabe..ef66496cc5 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -26,7 +26,7 @@ #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster:: IMUSensorBroadcaster { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 8d2f94ed33..f659af36db 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -23,7 +23,7 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index a4005a5583..ec58a423f1 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -179,6 +179,4 @@ The following version of the Joint Trajectory Controller are available mapping t .. - Input: position, [velocity, [acceleration]] .. - Output: effort -The controller uses `common hardware interface definitions`_. - (*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) 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 e09efc5026..fa7661ddb3 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 @@ -153,7 +153,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 bool use_closed_loop_pid_adapter = false; - // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 787ab72587..5e6f77b631 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -551,7 +551,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) node_->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - // TODO(karsten1987): no lifecyle for subscriber yet + // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher diff --git a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml index 6a497a09d1..09a134e06a 100644 --- a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml +++ b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml @@ -10,5 +10,3 @@ test_joint_trajectory_controller: state_interfaces: - position - velocity - - diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 42e57b7d49..d301d3bd2d 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -104,7 +104,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest }); // common_goal_response and common_result_response - // sometimes doesnt receive calls when we dont sleep + // sometimes doesn't receive calls when we don't sleep std::this_thread::sleep_for(std::chrono::milliseconds(300)); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0816dff5e6..44a88dfbe5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -811,7 +811,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set paramter + // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); @@ -896,7 +896,7 @@ TEST_P( TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set paramter + // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); @@ -983,7 +983,7 @@ 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 paramter + // 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 @@ -1031,7 +1031,7 @@ 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 paramter + // 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 @@ -1074,7 +1074,7 @@ TEST_P( } -// TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P +// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P // position controllers INSTANTIATE_TEST_CASE_P( diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0680a53c95..1b5f20dcc8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -167,7 +167,7 @@ class TrajectoryControllerTest : public ::testing::Test executor->add_node(traj_node_->get_node_base_interface()); } - // ignore velocity tolerances for this test since they arent commited in test_robot->write() + // 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_node_->set_parameter(stopped_velocity_parameters); diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 2d8acf3e09..88f35f207a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -41,4 +41,3 @@ Changelog for package position_controllers 0.1.0 (2020-12-23) ------------------ - diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index b3ccc1a4c1..21f5ea8d58 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -151,7 +151,7 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong numnber of joints + // send command with wrong number of joints auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index acb27dec43..773c3d6af3 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -28,7 +28,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::CommandInterface; -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController { FRIEND_TEST(JointGroupPositionControllerTest, CommandSuccessTest); diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b0812debf5..9b4bb47adc 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -40,4 +40,3 @@ Changelog for package ros2_controllers 0.1.0 (2020-12-23) ------------------ - diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ac1066d64a..8c682ef5f5 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -41,4 +41,3 @@ Changelog for package velocity_controllers 0.1.0 (2020-12-23) ------------------ - diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 36d0ff7a13..a3708c6e43 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -151,7 +151,7 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong numnber of joints + // send command with wrong number of joints auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 6d5f00fcdd..5b21ce8eae 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -29,7 +29,7 @@ using hardware_interface::HW_IF_VELOCITY; using hardware_interface::CommandInterface; -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController { FRIEND_TEST(JointGroupVelocityControllerTest, CommandSuccessTest); From d34800cf2a8621c84b4e2add5154d507cfd1f875 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Fri, 20 Aug 2021 06:49:45 -0400 Subject: [PATCH 007/524] Delete failing parameter undeclare in JointGroupPositionController (#222) * delete failing parameter unset from joint group position controller * explicitly set the interface_name parameter to HW_IF_POSITION * set interface_name param in velocity and effort controllers * fix line spacing and length * additional code style fix --- effort_controllers/src/joint_group_effort_controller.cpp | 6 ++++-- .../src/joint_group_position_controller.cpp | 6 ++++-- .../src/joint_group_velocity_controller.cpp | 6 ++++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 337b486c24..81f45de430 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -40,8 +40,10 @@ JointGroupEffortController::init( } try { - // undeclare interface parameter used in the general forward_command_controller - get_node()->undeclare_parameter("interface_name"); + // Explicitly set the interface parameter declared by the forward_command_controller + // to match the value set in the JointGroupEffortController constructor. + get_node()->set_parameter( + rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT)); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 4050817289..aef20ff8ce 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -39,8 +39,10 @@ JointGroupPositionController::init(const std::string & controller_name) } try { - // undeclare interface parameter used in the general forward_command_controller - get_node()->undeclare_parameter("interface_name"); + // Explicitly set the interface parameter declared by the forward_command_controller + // to match the value set in the JointGroupPositionController constructor. + get_node()->set_parameter( + rclcpp::Parameter("interface_name", hardware_interface::HW_IF_POSITION)); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 955e44d58f..0366b279ef 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -40,8 +40,10 @@ JointGroupVelocityController::init( } try { - // undeclare interface parameter used in the general forward_command_controller - get_node()->undeclare_parameter("interface_name"); + // Explicitly set the interface parameter declared by the forward_command_controller + // to match the value set in the JointGroupVelocityController constructor. + get_node()->set_parameter( + rclcpp::Parameter("interface_name", hardware_interface::HW_IF_VELOCITY)); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; From 33bfe4675782b96cd12673dddfd034fadd2aa9aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 25 Aug 2021 11:15:50 +0200 Subject: [PATCH 008/524] [Joint State Broadcaster] Add option to publish joint states to local topics (#218) * Use local topics for publishing joint states * Parameterize local namespace topics. * Unify test structure. * Use get_node() instead of node_ everywhere. * Add missing return in init method. * Update userdoc.rst --- joint_state_broadcaster/doc/userdoc.rst | 7 ++- .../joint_state_broadcaster.hpp | 6 +++ .../src/joint_state_broadcaster.cpp | 30 +++++++++++-- .../test/test_joint_state_broadcaster.cpp | 45 ++++++++++++++----- .../test/test_joint_state_broadcaster.hpp | 4 ++ 5 files changed, 76 insertions(+), 16 deletions(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 38c218266c..12749a41bf 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -18,5 +18,8 @@ All available *joint state interfaces* are used by this broadcaster. Parameters ---------- -extra_joints (optional; string array; default: empty) - Names of extra joints to be added to ``/joint_states`` and ``/dynamic_joint_states`` with state set to 0. +``use_local_topics`` + Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. + +``extra_joints`` + Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 62468e94af..49ff1734ca 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -36,6 +36,10 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface JOINT_STATE_BROADCASTER_PUBLIC JointStateBroadcaster(); + JOINT_STATE_BROADCASTER_PUBLIC + controller_interface::return_type + init(const std::string & controller_name) override; + JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -64,6 +68,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface void init_dynamic_joint_state_msg(); protected: + bool use_local_topics_; + // For the JointState message, // we store the name of joints with compatible interfaces std::vector joint_names_; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5e81409df1..9f7f91a7f4 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -48,6 +48,24 @@ using hardware_interface::HW_IF_EFFORT; JointStateBroadcaster::JointStateBroadcaster() {} +controller_interface::return_type +JointStateBroadcaster::init(const std::string & controller_name) +{ + auto ret = ControllerInterface::init(controller_name); + if (ret != controller_interface::return_type::OK) { + return ret; + } + + try { + get_node()->declare_parameter("use_local_topics", false); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + controller_interface::InterfaceConfiguration JointStateBroadcaster::command_interface_configuration() const { @@ -65,13 +83,17 @@ const rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); + try { + const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; + joint_state_publisher_ = get_node()->create_publisher( - "joint_states", rclcpp::SystemDefaultsQoS()); + topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); dynamic_joint_state_publisher_ = get_node()->create_publisher( - "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); + topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); } catch (const std::exception & e) { // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); @@ -209,8 +231,8 @@ JointStateBroadcaster::update() state_interface.get_value()); } - joint_state_msg_.header.stamp = node_->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = node_->get_clock()->now(); + joint_state_msg_.header.stamp = get_node()->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now(); // update joint state message and dynamic joint state message for (auto i = 0ul; i < joint_names_.size(); ++i) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index fdd65e6195..2eabe060a2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -115,7 +115,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); // configure failed - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception); SetUpStateBroadcaster(); // check state remains unchanged @@ -198,10 +198,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); } -TEST_F(JointStateBroadcasterTest, JointStatePublishTest) +void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) { - SetUpStateBroadcaster(); - auto node_state = state_broadcaster_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -213,7 +211,7 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest) { }; auto subscription = test_node.create_subscription( - "joint_states", + topic, 10, subs_callback); @@ -238,10 +236,24 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest) ASSERT_THAT(joint_state_msg.effort, ElementsAreArray(joint_state_msg.position)); } -TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) +TEST_F(JointStateBroadcasterTest, JointStatePublishTest) +{ + SetUpStateBroadcaster(); + + test_published_joint_state_message("joint_states"); +} + +TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) { SetUpStateBroadcaster(); + state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + + test_published_joint_state_message("joint_state_broadcaster/joint_states"); +} +void +JointStateBroadcasterTest::test_published_dynamic_joint_state_message(const std::string & topic) +{ auto node_state = state_broadcaster_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -253,7 +265,7 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) { }; auto subscription = test_node.create_subscription( - "dynamic_joint_states", + topic, 10, subs_callback); @@ -290,6 +302,21 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) } } +TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) +{ + SetUpStateBroadcaster(); + + test_published_dynamic_joint_state_message("dynamic_joint_states"); +} + +TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) +{ + SetUpStateBroadcaster(); + state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + + test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states"); +} + TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) { // joint state not initialized yet @@ -309,10 +336,8 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) SetUpStateBroadcaster(); // Add extra joints as parameters - auto broadcaster_node = state_broadcaster_->get_node(); const std::vector extra_joint_names = {"extra1", "extra2", "extra3"}; - const rclcpp::Parameter extra_joints_parameters("extra_joints", extra_joint_names); - broadcaster_node->set_parameter(extra_joints_parameters); + state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names}); // configure ok ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index f659af36db..dc04f10486 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -42,6 +42,10 @@ class JointStateBroadcasterTest : public ::testing::Test void SetUpStateBroadcaster(); + void test_published_joint_state_message(const std::string & topic); + + void test_published_dynamic_joint_state_message(const std::string & topic); + protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; From 3bf219bc8890c564dd4c6c41fa6b5f81df682b40 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Wed, 25 Aug 2021 14:48:16 -0500 Subject: [PATCH 009/524] Copy over GitHub templates from ros2_control (#228) * docs: :memo: Port over ros2_control GitHub templates * This first commit is an exact copy. Next commit will be any changes to adapt to ros2_controllers * docs: :memo: Convert ros2_control references to ros2_controller --- .github/ISSUE_TEMPLATE/bug_report.md | 32 ++++++++++ .github/ISSUE_TEMPLATE/feature_request.md | 20 +++++++ .github/ISSUE_TEMPLATE/good-first-issue.md | 60 +++++++++++++++++++ .../pull_request_template.md | 26 ++++++++ 4 files changed, 138 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md create mode 100644 .github/ISSUE_TEMPLATE/good-first-issue.md create mode 100644 .github/PULL_REQUEST_TEMPLATE/pull_request_template.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000000..20072278df --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,32 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Environment (please complete the following information):** + - OS: [e.g. iOS] + - Version [e.g. Foxy] +- Anything that may be unusual about your environment + +**Additional context** +Add any other context about the problem here, especially include any modifications to any ros2_controllers that relate to this issue. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..11fc491ef1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for this project +title: '' +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md new file mode 100644 index 0000000000..6bbf2e957a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -0,0 +1,60 @@ +--- +name: Good first issue +about: Create an issue to welcome a new contributor into the community. +title: '' +labels: good-first-issue +assignees: '' + +--- + +## Background + +Overview of your issue here. + +## Instructions +Hi, this is a `good-first-issue` issue. This means we've worked to make it more legible to people who either **haven't contributed to our codebase before, or even folks who haven't contributed to open source before**. + +We're interested in helping you take the first step, and can answer questions and help you out along the way. Note that we're especially interested in contributions from underrepresented groups! + +We know that creating a pull request is the biggest barrier for new contributors. This issue is for you 💝 + +If you have contributed before, **consider leaving this PR for someone new**, and looking through our general [bug](https://github.com/ros-controls/ros2_controllers/labels/bug) issues. Thanks! + +### 🤔 What you will need to know. + +Nothing. This issue is meant to welcome you to Open Source :) We are happy to walk you through the process. + +### 📋 Step by Step + +- [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! + +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). + +- [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) + +- [ ] **Checkout a new branch** using `git checkout -b ` + +- [ ] 🤖 **Apply `pre-commit`** auto formatting, by running `pip3 install pre-commit` and running `pre-commit install` in the ros2_control repo. + +- [ ] 💾 **Commit and Push** your changes + +- [ ] 🔀 **Start a Pull Request** to request to merge your code into `master`. There are two ways that you can start a pull request: +1. If you are not familiar with GitHub or how to create a pull request, [here is a guide you can follow](https://guides.github.com/activities/hello-world/) on how GitHub works. + +- [ ] 🏁 **Done** Ask in comments for a review :) + +### Is someone else already working on this? + +🔗- We encourage contributors to link to the original issue in their pull request so all users can easily see if someone's already started on it. + +👥- **If someone seems stuck, offer them some help!** + +### 🤔❓ Questions? + +Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! +Furthermore, you find helpful resources here: +* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) +* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html) +* [ROS Answers](https://answers.ros.org/questions/) + +**Good luck with your first issue!** diff --git a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md b/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md new file mode 100644 index 0000000000..87f389a124 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md @@ -0,0 +1,26 @@ + +--- +name: Pull request +about: Create a pull request +title: '' +labels: '' +assignees: '' + +--- + +Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: + +1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs. +2. Give your PR a descriptive title. Add a short summary, if required. +3. Make sure the pipeline is green. +4. Don’t be afraid to request reviews from maintainers. +5. New code = new tests. If you are adding new functionality, always make sure to add some tests exercising the code and serving as live documentation of your original intention. + +To send us a pull request, please: + +- [ ] Fork the repository. +- [ ] Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change. +- [ ] Ensure local tests pass. (`colcon test` and `pre-commit run` (requires you to install pre-commit by `pip3 install pre-commit`) +- [ ] Commit to your fork using clear commit messages. +- [ ] Send a pull request, answering any default questions in the pull request interface. +- [ ] Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. From 3c61e4f9177123e33929dce47fec17bc918f2379 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 26 Aug 2021 08:15:14 +0100 Subject: [PATCH 010/524] Bring precommit config up to speed with ros2_control (#227) * Adjust ROS linters job * No linting as tests * Replace uncrustify with clang format * Apply reformat --- .clang-format | 15 + .github/workflows/lint.yml | 31 +- .pre-commit-config.yaml | 12 +- diff_drive_controller/CMakeLists.txt | 3 - .../diff_drive_controller.hpp | 37 +- .../diff_drive_controller/odometry.hpp | 29 +- .../rolling_mean_accumulator.hpp | 6 +- .../diff_drive_controller/speed_limiter.hpp | 6 +- .../visibility_control.h | 48 +- diff_drive_controller/package.xml | 2 - .../src/diff_drive_controller.cpp | 274 ++++---- diff_drive_controller/src/odometry.cpp | 15 +- diff_drive_controller/src/speed_limiter.cpp | 46 +- .../test/test_accumulator.cpp | 4 +- .../test/test_diff_drive_controller.cpp | 127 ++-- .../test/test_load_diff_drive_controller.cpp | 10 +- effort_controllers/CMakeLists.txt | 3 - .../joint_group_effort_controller.hpp | 9 +- .../effort_controllers/visibility_control.h | 48 +- effort_controllers/package.xml | 2 - .../src/joint_group_effort_controller.cpp | 18 +- .../test_joint_group_effort_controller.cpp | 29 +- .../test_joint_group_effort_controller.hpp | 4 +- ...est_load_joint_group_effort_controller.cpp | 9 +- .../force_torque_sensor_broadcaster.hpp | 2 +- .../src/force_torque_sensor_broadcaster.cpp | 59 +- .../test_force_torque_sensor_broadcaster.cpp | 26 +- .../test_force_torque_sensor_broadcaster.hpp | 4 +- ...t_load_force_torque_sensor_broadcaster.cpp | 10 +- forward_command_controller/CMakeLists.txt | 3 - .../forward_command_controller.hpp | 2 +- .../visibility_control.h | 48 +- forward_command_controller/package.xml | 2 - .../src/forward_command_controller.cpp | 71 +- .../test/test_forward_command_controller.cpp | 30 +- .../test/test_forward_command_controller.hpp | 2 +- .../test_load_forward_command_controller.cpp | 9 +- gripper_controllers/CMakeLists.txt | 3 - .../gripper_action_controller.hpp | 65 +- .../gripper_action_controller_impl.hpp | 212 +++--- .../hardware_interface_adapter.hpp | 57 +- .../visibility_control.hpp | 34 +- gripper_controllers/package.xml | 2 - .../src/gripper_action_controller.cpp | 12 +- .../test_load_gripper_action_controllers.cpp | 15 +- imu_sensor_broadcaster/CMakeLists.txt | 3 - .../imu_sensor_broadcaster.hpp | 4 +- .../src/imu_sensor_broadcaster.cpp | 55 +- .../test/test_imu_sensor_broadcaster.cpp | 35 +- .../test/test_imu_sensor_broadcaster.hpp | 44 +- .../test/test_load_imu_sensor_broadcaster.cpp | 9 +- joint_state_broadcaster/CMakeLists.txt | 3 - .../joint_state_broadcaster.hpp | 24 +- .../visibility_control.h | 48 +- joint_state_broadcaster/package.xml | 2 - .../src/joint_state_broadcaster.cpp | 116 +-- .../test/test_joint_state_broadcaster.cpp | 62 +- .../test/test_joint_state_broadcaster.hpp | 36 +- .../test_load_joint_state_broadcaster.cpp | 12 +- joint_state_controller/CMakeLists.txt | 3 - joint_state_controller/package.xml | 2 - .../test/test_load_joint_state_controller.cpp | 12 +- joint_trajectory_controller/CMakeLists.txt | 3 - .../joint_trajectory_controller.hpp | 67 +- .../tolerances.hpp | 73 +- .../trajectory.hpp | 64 +- .../visibility_control.h | 48 +- joint_trajectory_controller/package.xml | 2 - .../src/joint_trajectory_controller.cpp | 665 ++++++++++-------- .../src/trajectory.cpp | 179 +++-- .../test_load_joint_trajectory_controller.cpp | 11 +- .../test/test_trajectory.cpp | 43 +- .../test/test_trajectory_actions.cpp | 154 ++-- .../test/test_trajectory_controller.cpp | 248 +++---- .../test/test_trajectory_controller_utils.hpp | 161 ++--- position_controllers/CMakeLists.txt | 3 - .../joint_group_position_controller.hpp | 5 +- .../position_controllers/visibility_control.h | 48 +- position_controllers/package.xml | 2 - .../src/joint_group_position_controller.cpp | 14 +- .../test_joint_group_position_controller.cpp | 28 +- .../test_joint_group_position_controller.hpp | 2 +- ...t_load_joint_group_position_controller.cpp | 9 +- velocity_controllers/CMakeLists.txt | 3 - .../joint_group_velocity_controller.hpp | 7 +- .../velocity_controllers/visibility_control.h | 48 +- velocity_controllers/package.xml | 2 - .../src/joint_group_velocity_controller.cpp | 18 +- .../test_joint_group_velocity_controller.cpp | 29 +- .../test_joint_group_velocity_controller.hpp | 3 +- ...t_load_joint_group_velocity_controller.cpp | 9 +- 91 files changed, 1873 insertions(+), 2000 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..9c21e1e17c --- /dev/null +++ b/.clang-format @@ -0,0 +1,15 @@ +--- +Language: Cpp +BasedOnStyle: Google + +ColumnLimit: 100 +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BreakBeforeBraces: Allman +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +IncludeBlocks: Preserve +... diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index f9d7d6aade..d2f266d3fa 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -1,5 +1,4 @@ -# Run linters automatically on pull requests. -name: Lint ros2_controllers +name: ROS Lint on: pull_request: @@ -10,7 +9,7 @@ jobs: strategy: fail-fast: false matrix: - linter: [copyright, cppcheck, cpplint, flake8, pep257, uncrustify, xmllint] + linter: [copyright, cppcheck, lint_cmake] steps: - uses: actions/checkout@v1 - uses: ros-tooling/setup-ros@v0.2 @@ -30,3 +29,29 @@ jobs: position_controllers ros2_controllers velocity_controllers + + ament_lint_cpplint: + name: ament_lint_cpplint + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + steps: + - uses: actions/checkout@v1 + - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: foxy + linter: cpplint + arguments: "--filter=-whitespace/newline" + package-name: + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_state_controller + joint_trajectory_controller + gripper_controllers + position_controllers + ros2_controllers + velocity_controllers diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 16acfea157..70116088ce 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -55,13 +55,13 @@ repos: # CPP hooks - repo: local hooks: - - id: ament_uncrustify - name: ament_uncrustify - description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-10 language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] - repo: local hooks: diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index c1e1807a86..d359d90968 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -61,11 +61,8 @@ install(TARGETS diff_drive_controller if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock(test_diff_drive_controller test/test_diff_drive_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) 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 e64e571d9d..e9c07114dc 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -30,9 +30,9 @@ #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" -#include "hardware_interface/handle.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp/rclcpp.hpp" @@ -42,7 +42,6 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" - namespace diff_drive_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -56,16 +55,13 @@ class DiffDriveController : public controller_interface::ControllerInterface DiffDriveController(); DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update() override; @@ -96,8 +92,7 @@ class DiffDriveController : public controller_interface::ControllerInterface }; CallbackReturn configure_side( - const std::string & side, - const std::vector & wheel_names, + const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); std::vector left_wheel_names_; @@ -109,8 +104,8 @@ class DiffDriveController : public controller_interface::ControllerInterface struct WheelParams { size_t wheels_per_side = 0; - double separation = 0.0; // w.r.t. the midpoint of the wheel width - double radius = 0.0; // Assumed to be the same for both wheels + double separation = 0.0; // w.r.t. the midpoint of the wheel width + double radius = 0.0; // Assumed to be the same for both wheels double separation_multiplier = 1.0; double left_radius_multiplier = 1.0; double right_radius_multiplier = 1.0; @@ -130,21 +125,20 @@ class DiffDriveController : public controller_interface::ControllerInterface std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> - realtime_odometry_publisher_ = nullptr; + realtime_odometry_publisher_ = nullptr; - std::shared_ptr> - odometry_transform_publisher_ = nullptr; + std::shared_ptr> odometry_transform_publisher_ = + nullptr; std::shared_ptr> - realtime_odometry_transform_publisher_ = nullptr; + realtime_odometry_transform_publisher_ = nullptr; // Timeout to consider cmd_vel commands old std::chrono::milliseconds cmd_vel_timeout_{500}; bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ - = - nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; @@ -155,10 +149,9 @@ class DiffDriveController : public controller_interface::ControllerInterface SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> realtime_limited_velocity_publisher_ = nullptr; - std::shared_ptr> - realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 25747a5b59..57af643bf0 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -29,7 +29,6 @@ namespace diff_drive_controller { - class Odometry { public: @@ -40,33 +39,17 @@ class Odometry void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); void resetOdometry(); - double getX() const - { - return x_; - } - double getY() const - { - return y_; - } - double getHeading() const - { - return heading_; - } - double getLinear() const - { - return linear_; - } - double getAngular() const - { - return angular_; - } + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius); void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = - diff_drive_controller::RollingMeanAccumulator; + using RollingMeanAccumulator = diff_drive_controller::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp b/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp index cc4400896e..c91e4f7eff 100644 --- a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp +++ b/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp @@ -24,20 +24,18 @@ namespace diff_drive_controller { - /** * \brief Simplification of boost::accumulators::accumulator_set> to avoid dragging boost dependencies * * Computes the mean of the last accumulated elements */ -template +template class RollingMeanAccumulator { public: explicit RollingMeanAccumulator(size_t rolling_window_size) - : buffer_(rolling_window_size, 0.0), next_insert_(0), - sum_(0.0), buffer_filled_(false) + : buffer_(rolling_window_size, 0.0), next_insert_(0), sum_(0.0), buffer_filled_(false) { } diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 7e6fe4a7e9..f6fe439f5d 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -40,9 +40,9 @@ class SpeedLimiter */ SpeedLimiter( bool has_velocity_limits = false, bool has_acceleration_limits = false, - bool has_jerk_limits = false, - double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, - double max_acceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN); /** * \brief Limit the velocity and acceleration diff --git a/diff_drive_controller/include/diff_drive_controller/visibility_control.h b/diff_drive_controller/include/diff_drive_controller/visibility_control.h index a6b3e5228b..94d78153eb 100644 --- a/diff_drive_controller/include/diff_drive_controller/visibility_control.h +++ b/diff_drive_controller/include/diff_drive_controller/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__ ((dllexport)) - #define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__ ((dllimport)) - #else - #define DIFF_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) - #define DIFF_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) - #endif - #ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL - #define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT - #else - #define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT - #endif - #define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC - #define DIFF_DRIVE_CONTROLLER_LOCAL +#ifdef __GNUC__ +#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__((dllimport)) #else - #define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) - #define DIFF_DRIVE_CONTROLLER_IMPORT - #if __GNUC__ >= 4 - #define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) - #define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define DIFF_DRIVE_CONTROLLER_PUBLIC - #define DIFF_DRIVE_CONTROLLER_LOCAL - #endif - #define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE +#define DIFF_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) +#define DIFF_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL +#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT +#else +#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT +#endif +#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC +#define DIFF_DRIVE_CONTROLLER_LOCAL +#else +#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define DIFF_DRIVE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define DIFF_DRIVE_CONTROLLER_PUBLIC +#define DIFF_DRIVE_CONTROLLER_LOCAL +#endif +#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE #endif #endif // DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0b5d6eeb6d..f2f622084d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -23,8 +23,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a4ddbbbc6a..46f074b852 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -23,10 +23,10 @@ #include #include "diff_drive_controller/diff_drive_controller.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/LinearMath/Quaternion.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" namespace { @@ -40,25 +40,25 @@ constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; namespace diff_drive_controller { using namespace std::chrono_literals; -using lifecycle_msgs::msg::State; -using controller_interface::InterfaceConfiguration; using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() -: controller_interface::ControllerInterface() {} +DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} -controller_interface::return_type -DiffDriveController::init(const std::string & controller_name) +controller_interface::return_type DiffDriveController::init(const std::string & controller_name) { // initialize lifecycle node auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { auto node = get_node(); // with the lifecycle node being initialized, we can declare parameters node->declare_parameter>( @@ -70,14 +70,11 @@ DiffDriveController::init(const std::string & controller_name) node->declare_parameter("wheels_per_side", wheel_params_.wheels_per_side); node->declare_parameter("wheel_radius", wheel_params_.radius); node->declare_parameter( - "wheel_separation_multiplier", - wheel_params_.separation_multiplier); + "wheel_separation_multiplier", wheel_params_.separation_multiplier); node->declare_parameter( - "left_wheel_radius_multiplier", - wheel_params_.left_radius_multiplier); + "left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); node->declare_parameter( - "right_wheel_radius_multiplier", - wheel_params_.right_radius_multiplier); + "right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); node->declare_parameter("odom_frame_id", odom_params_.odom_frame_id); node->declare_parameter("base_frame_id", odom_params_.base_frame_id); @@ -111,7 +108,9 @@ DiffDriveController::init(const std::string & controller_name) node->declare_parameter("angular.z.min_acceleration", NAN); node->declare_parameter("angular.z.max_jerk", NAN); node->declare_parameter("angular.z.min_jerk", NAN); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -122,10 +121,12 @@ DiffDriveController::init(const std::string & controller_name) InterfaceConfiguration DiffDriveController::command_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) { + for (const auto & joint_name : left_wheel_names_) + { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } - for (const auto & joint_name : right_wheel_names_) { + for (const auto & joint_name : right_wheel_names_) + { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } return {interface_configuration_type::INDIVIDUAL, conf_names}; @@ -134,21 +135,24 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co InterfaceConfiguration DiffDriveController::state_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) { + for (const auto & joint_name : left_wheel_names_) + { conf_names.push_back(joint_name + "/" + HW_IF_POSITION); } - for (const auto & joint_name : right_wheel_names_) { + for (const auto & joint_name : right_wheel_names_) + { conf_names.push_back(joint_name + "/" + HW_IF_POSITION); } return {interface_configuration_type::INDIVIDUAL, conf_names}; } - controller_interface::return_type DiffDriveController::update() { auto logger = node_->get_logger(); - if (get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { - if (!is_halted) { + if (get_current_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { halt(); is_halted = true; } @@ -160,14 +164,16 @@ controller_interface::return_type DiffDriveController::update() std::shared_ptr last_msg; received_velocity_msg_ptr_.get(last_msg); - if (last_msg == nullptr) { + if (last_msg == nullptr) + { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } const auto dt = current_time - last_msg->header.stamp; // Brake if cmd_vel has timeout, override the stored command - if (dt > cmd_vel_timeout_) { + if (dt > cmd_vel_timeout_) + { last_msg->twist.linear.x = 0.0; last_msg->twist.angular.z = 0.0; } @@ -183,20 +189,24 @@ controller_interface::return_type DiffDriveController::update() const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; - if (odom_params_.open_loop) { + if (odom_params_.open_loop) + { odometry_.updateOpenLoop(linear_command, angular_command, current_time); - } else { + } + else + { double left_position_mean = 0.0; double right_position_mean = 0.0; - for (size_t index = 0; index < wheels.wheels_per_side; ++index) { + for (size_t index = 0; index < wheels.wheels_per_side; ++index) + { const double left_position = registered_left_wheel_handles_[index].position.get().get_value(); const double right_position = registered_right_wheel_handles_[index].position.get().get_value(); - if (std::isnan(left_position) || std::isnan(right_position)) { + if (std::isnan(left_position) || std::isnan(right_position)) + { RCLCPP_ERROR( - logger, "Either the left or right wheel position is invalid for index [%zu]", - index); + logger, "Either the left or right wheel position is invalid for index [%zu]", index); return controller_interface::return_type::ERROR; } @@ -212,7 +222,8 @@ controller_interface::return_type DiffDriveController::update() tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (realtime_odometry_publisher_->trylock()) { + if (realtime_odometry_publisher_->trylock()) + { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = current_time; odometry_message.pose.pose.position.x = odometry_.getX(); @@ -226,7 +237,8 @@ controller_interface::return_type DiffDriveController::update() realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = current_time; transform.transform.translation.x = odometry_.getX(); @@ -238,15 +250,13 @@ controller_interface::return_type DiffDriveController::update() realtime_odometry_transform_publisher_->unlockAndPublish(); } - const auto update_dt = current_time - previous_update_timestamp_; previous_update_timestamp_ = current_time; auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; limiter_linear_.limit( - linear_command, last_command.linear.x, second_to_last_command.linear.x, - update_dt.seconds()); + linear_command, last_command.linear.x, second_to_last_command.linear.x, update_dt.seconds()); limiter_angular_.limit( angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); @@ -254,7 +264,8 @@ controller_interface::return_type DiffDriveController::update() previous_commands_.emplace(*last_msg); // Publish limited velocity - if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { + if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) + { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; limited_velocity_command.header.stamp = current_time; limited_velocity_command.twist.linear.x = linear_command; @@ -263,13 +274,14 @@ controller_interface::return_type DiffDriveController::update() } // Compute wheels velocities: - const double velocity_left = (linear_command - angular_command * wheel_separation / 2.0) / - left_wheel_radius; - const double velocity_right = (linear_command + angular_command * wheel_separation / 2.0) / - right_wheel_radius; + const double velocity_left = + (linear_command - angular_command * wheel_separation / 2.0) / left_wheel_radius; + const double velocity_right = + (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < wheels.wheels_per_side; ++index) { + for (size_t index = 0; index < wheels.wheels_per_side; ++index) + { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); } @@ -285,16 +297,16 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & left_wheel_names_ = node_->get_parameter("left_wheel_names").as_string_array(); right_wheel_names_ = node_->get_parameter("right_wheel_names").as_string_array(); - if (left_wheel_names_.size() != right_wheel_names_.size()) { + if (left_wheel_names_.size() != right_wheel_names_.size()) + { RCLCPP_ERROR( - logger, - "The number of left wheels [%zu] and the number of right wheels [%zu] are different", - left_wheel_names_.size(), - right_wheel_names_.size()); + logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", + left_wheel_names_.size(), right_wheel_names_.size()); return CallbackReturn::ERROR; } - if (left_wheel_names_.empty()) { + if (left_wheel_names_.empty()) + { RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); return CallbackReturn::ERROR; } @@ -305,10 +317,10 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & wheel_params_.radius = node_->get_parameter("wheel_radius").as_double(); wheel_params_.separation_multiplier = node_->get_parameter("wheel_separation_multiplier").as_double(); - wheel_params_.left_radius_multiplier = node_->get_parameter( - "left_wheel_radius_multiplier").as_double(); - wheel_params_.right_radius_multiplier = node_->get_parameter( - "right_wheel_radius_multiplier").as_double(); + wheel_params_.left_radius_multiplier = + node_->get_parameter("left_wheel_radius_multiplier").as_double(); + wheel_params_.right_radius_multiplier = + node_->get_parameter("right_wheel_radius_multiplier").as_double(); const auto wheels = wheel_params_; @@ -318,33 +330,29 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize( - node_->get_parameter( - "velocity_rolling_window_size").as_int()); + node_->get_parameter("velocity_rolling_window_size").as_int()); odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); std::copy( - pose_diagonal.begin(), pose_diagonal.end(), - odom_params_.pose_covariance_diagonal.begin()); + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - auto twist_diagonal = - node_->get_parameter("twist_covariance_diagonal").as_double_array(); + auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); std::copy( - twist_diagonal.begin(), - twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); - cmd_vel_timeout_ = - std::chrono::milliseconds{static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * - 1000.0)}; + cmd_vel_timeout_ = std::chrono::milliseconds{ + static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; publish_limited_velocity_ = node_->get_parameter("publish_limited_velocity").as_bool(); use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); - try { + try + { limiter_linear_ = SpeedLimiter( node_->get_parameter("linear.x.has_velocity_limits").as_bool(), node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), @@ -355,11 +363,14 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & node_->get_parameter("linear.x.max_acceleration").as_double(), node_->get_parameter("linear.x.min_jerk").as_double(), node_->get_parameter("linear.x.max_jerk").as_double()); - } catch (const std::runtime_error & e) { + } + catch (const std::runtime_error & e) + { RCLCPP_ERROR(node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); } - try { + try + { limiter_angular_ = SpeedLimiter( node_->get_parameter("angular.z.has_velocity_limits").as_bool(), node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), @@ -370,23 +381,24 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & node_->get_parameter("angular.z.max_acceleration").as_double(), node_->get_parameter("angular.z.min_jerk").as_double(), node_->get_parameter("angular.z.max_jerk").as_double()); - } catch (const std::runtime_error & e) { + } + catch (const std::runtime_error & e) + { RCLCPP_ERROR(node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } - - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } // left and right sides are both equal at this point wheel_params_.wheels_per_side = left_wheel_names_.size(); - if (publish_limited_velocity_) { + if (publish_limited_velocity_) + { limited_velocity_publisher_ = - node_->create_publisher( - DEFAULT_COMMAND_OUT_TOPIC, - rclcpp::SystemDefaultsQoS()); + node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = std::make_shared>(limited_velocity_publisher_); } @@ -399,17 +411,18 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & previous_commands_.emplace(empty_twist); // initialize command subscriber - if (use_stamped_vel_) { + if (use_stamped_vel_) + { velocity_command_subscriber_ = node_->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this]( - const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) { - RCLCPP_WARN( - node_->get_logger(), - "Can't accept new commands. subscriber is inactive"); + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { RCLCPP_WARN_ONCE( node_->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " @@ -418,15 +431,15 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } received_velocity_msg_ptr_.set(std::move(msg)); }); - } else { - velocity_command_unstamped_subscriber_ = - node_->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), [this]( - const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) { - RCLCPP_WARN( - node_->get_logger(), - "Can't accept new commands. subscriber is inactive"); + } + else + { + velocity_command_unstamped_subscriber_ = node_->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } @@ -438,26 +451,24 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & }); } - // initialize odometry publisher and messasge - odometry_publisher_ = - node_->create_publisher( - DEFAULT_ODOMETRY_TOPIC, - rclcpp::SystemDefaultsQoS()); + odometry_publisher_ = node_->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = - std::make_shared< - realtime_tools::RealtimePublisher>(odometry_publisher_); + std::make_shared>( + odometry_publisher_); auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; // initialize odom values zeros - odometry_message.twist = geometry_msgs::msg::TwistWithCovariance( - rosidl_runtime_cpp::MessageInitialization::ALL); + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; - for (size_t index = 0; index < 6; ++index) { + for (size_t index = 0; index < 6; ++index) + { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; @@ -466,13 +477,11 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } // initialize transform publisher and message - odometry_transform_publisher_ = - node_->create_publisher( - DEFAULT_TRANSFORM_TOPIC, - rclcpp::SystemDefaultsQoS()); + odometry_transform_publisher_ = node_->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_transform_publisher_ = std::make_shared>( - odometry_transform_publisher_); + odometry_transform_publisher_); // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; @@ -491,14 +500,15 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) const auto right_result = configure_side("right", right_wheel_names_, registered_right_wheel_handles_); - if (left_result == CallbackReturn::ERROR || right_result == CallbackReturn::ERROR) { + if (left_result == CallbackReturn::ERROR || right_result == CallbackReturn::ERROR) + { return CallbackReturn::ERROR; } - if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { + if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) + { RCLCPP_ERROR( - node_->get_logger(), - "Either left wheel interfaces, right wheel interfaces are non existent"); + node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); return CallbackReturn::ERROR; } @@ -517,7 +527,8 @@ CallbackReturn DiffDriveController::on_deactivate(const rclcpp_lifecycle::State CallbackReturn DiffDriveController::on_cleanup(const rclcpp_lifecycle::State &) { - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } @@ -527,7 +538,8 @@ CallbackReturn DiffDriveController::on_cleanup(const rclcpp_lifecycle::State &) CallbackReturn DiffDriveController::on_error(const rclcpp_lifecycle::State &) { - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; @@ -561,56 +573,59 @@ CallbackReturn DiffDriveController::on_shutdown(const rclcpp_lifecycle::State &) void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) { - for (const auto & wheel_handle : wheel_handles) { - wheel_handle.velocity.get().set_value(0.0); - } - }; + for (const auto & wheel_handle : wheel_handles) + { + wheel_handle.velocity.get().set_value(0.0); + } + }; halt_wheels(registered_left_wheel_handles_); halt_wheels(registered_right_wheel_handles_); } CallbackReturn DiffDriveController::configure_side( - const std::string & side, - const std::vector & wheel_names, + const std::string & side, const std::vector & wheel_names, std::vector & registered_handles) { auto logger = node_->get_logger(); - if (wheel_names.empty()) { + if (wheel_names.empty()) + { RCLCPP_ERROR(logger, "No '%s' wheel names specified", side.c_str()); return CallbackReturn::ERROR; } // register handles registered_handles.reserve(wheel_names.size()); - for (const auto & wheel_name : wheel_names) { + for (const auto & wheel_name : wheel_names) + { const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name](const auto & interface) { return interface.get_name() == wheel_name && - interface.get_interface_name() == HW_IF_POSITION; + interface.get_interface_name() == HW_IF_POSITION; }); - if (state_handle == state_interfaces_.cend()) { + if (state_handle == state_interfaces_.cend()) + { RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); return CallbackReturn::ERROR; } const auto command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name]( - const auto & interface) { + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) { return interface.get_name() == wheel_name && - interface.get_interface_name() == HW_IF_VELOCITY; + interface.get_interface_name() == HW_IF_VELOCITY; }); - if (command_handle == command_interfaces_.end()) { + if (command_handle == command_interfaces_.end()) + { RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); return CallbackReturn::ERROR; } registered_handles.emplace_back( - WheelHandle{std::ref(*state_handle), - std::ref(*command_handle)}); + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); } return CallbackReturn::SUCCESS; @@ -620,5 +635,4 @@ CallbackReturn DiffDriveController::configure_side( #include "class_loader/register_macro.hpp" CLASS_LOADER_REGISTER_CLASS( - diff_drive_controller::DiffDriveController, - controller_interface::ControllerInterface) + diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index d36cb037ee..34168fb85e 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -49,8 +49,9 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti { // We cannot estimate the speed with very small time intervals: const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) { - return false; // Interval too small to integrate with + if (dt < 0.0001) + { + return false; // Interval too small to integrate with } // Get current wheel joint positions: @@ -105,8 +106,7 @@ void Odometry::resetOdometry() } void Odometry::setWheelParams( - double wheel_separation, double left_wheel_radius, - double right_wheel_radius) + double wheel_separation, double left_wheel_radius, double right_wheel_radius) { wheel_separation_ = wheel_separation; left_wheel_radius_ = left_wheel_radius; @@ -132,9 +132,12 @@ void Odometry::integrateRungeKutta2(double linear, double angular) void Odometry::integrateExact(double linear, double angular) { - if (fabs(angular) < 1e-6) { + if (fabs(angular) < 1e-6) + { integrateRungeKutta2(linear, angular); - } else { + } + else + { /// Exact integration (should solve problems when angular is zero): const double heading_old = heading_; const double r = linear / angular; diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp index a12adde53a..e25a6f47ed 100644 --- a/diff_drive_controller/src/speed_limiter.cpp +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -25,10 +25,10 @@ namespace diff_drive_controller { SpeedLimiter::SpeedLimiter( - bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, - double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, - double min_jerk, double max_jerk) -: has_velocity_limits_(has_velocity_limits), + bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, + double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, + double max_jerk) +: has_velocity_limits_(has_velocity_limits), has_acceleration_limits_(has_acceleration_limits), has_jerk_limits_(has_jerk_limits), min_velocity_(min_velocity), @@ -39,28 +39,37 @@ SpeedLimiter::SpeedLimiter( max_jerk_(max_jerk) { // Check if limits are valid, max must be specified, min defaults to -max if unspecified - if (has_velocity_limits_) { - if (std::isnan(max_velocity_)) { + if (has_velocity_limits_) + { + if (std::isnan(max_velocity_)) + { throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); } - if (std::isnan(min_velocity_)) { + if (std::isnan(min_velocity_)) + { min_velocity_ = -max_velocity_; } } - if (has_acceleration_limits_) { - if (std::isnan(max_acceleration_)) { + if (has_acceleration_limits_) + { + if (std::isnan(max_acceleration_)) + { throw std::runtime_error( - "Cannot apply acceleration limits if max_acceleration is not specified"); + "Cannot apply acceleration limits if max_acceleration is not specified"); } - if (std::isnan(min_acceleration_)) { + if (std::isnan(min_acceleration_)) + { min_acceleration_ = -max_acceleration_; } } - if (has_jerk_limits_) { - if (std::isnan(max_jerk_)) { + if (has_jerk_limits_) + { + if (std::isnan(max_jerk_)) + { throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); } - if (std::isnan(min_jerk_)) { + if (std::isnan(min_jerk_)) + { min_jerk_ = -max_jerk_; } } @@ -81,7 +90,8 @@ double SpeedLimiter::limit_velocity(double & v) { const double tmp = v; - if (has_velocity_limits_) { + if (has_velocity_limits_) + { v = rcppmath::clamp(v, min_velocity_, max_velocity_); } @@ -92,7 +102,8 @@ double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) { const double tmp = v; - if (has_acceleration_limits_) { + if (has_acceleration_limits_) + { const double dv_min = min_acceleration_ * dt; const double dv_max = max_acceleration_ * dt; @@ -108,7 +119,8 @@ double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) { const double tmp = v; - if (has_jerk_limits_) { + if (has_jerk_limits_) + { const double dv = v - v0; const double dv0 = v0 - v1; diff --git a/diff_drive_controller/test/test_accumulator.cpp b/diff_drive_controller/test/test_accumulator.cpp index a80b17c27b..4926974b8e 100644 --- a/diff_drive_controller/test/test_accumulator.cpp +++ b/diff_drive_controller/test/test_accumulator.cpp @@ -21,7 +21,6 @@ #include #include - TEST(TestAccumulator, test_accumulator) { constexpr double THRESHOLD = 1e-12; @@ -51,7 +50,8 @@ TEST(TestAccumulator, spam_accumulator) { constexpr double THRESHOLD = 1e-12; diff_drive_controller::RollingMeanAccumulator accum(10); - for (int i = 0; i < 10000; ++i) { + for (int i = 0; i < 10000; ++i) + { accum.accumulate(M_PI); EXPECT_NEAR(M_PI, accum.getRollingMean(), THRESHOLD); } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4138c738e8..a3dda0b087 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -18,26 +18,24 @@ #include #include #include -#include #include +#include #include "diff_drive_controller/diff_drive_controller.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" -#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" - +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" using CallbackReturn = diff_drive_controller::CallbackReturn; -using lifecycle_msgs::msg::State; -using hardware_interface::LoanedStateInterface; -using hardware_interface::LoanedCommandInterface; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; using testing::SizeIs; - class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -63,7 +61,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr rclcpp::WaitSet wait_set; wait_set.add_subscription(velocity_command_subscriber_); - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) { + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { executor.spin_some(); return true; } @@ -74,10 +73,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr class TestDiffDriveController : public ::testing::Test { protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } void SetUp() override { @@ -85,14 +81,10 @@ class TestDiffDriveController : public ::testing::Test pub_node = std::make_shared("velocity_publisher"); velocity_publisher = pub_node->create_publisher( - controller_name + "/cmd_vel", - rclcpp::SystemDefaultsQoS()); + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() - { - rclcpp::shutdown(); - } + static void TearDownTestCase() { rclcpp::shutdown(); } /// Publish velocity msgs /** @@ -103,8 +95,10 @@ class TestDiffDriveController : public ::testing::Test { int wait_count = 0; auto topic = velocity_publisher->get_topic_name(); - while (pub_node->count_subscribers(topic) == 0) { - if (wait_count >= 5) { + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; throw std::runtime_error(error_msg); } @@ -125,8 +119,10 @@ class TestDiffDriveController : public ::testing::Test constexpr std::chrono::seconds TIMEOUT{2}; auto clock = pub_node->get_clock(); auto start = clock->now(); - while (velocity_publisher->get_subscription_count() <= 0) { - if ((clock->now() - start) > TIMEOUT) { + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { FAIL(); } rclcpp::spin_some(pub_node); @@ -146,7 +142,6 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } - const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; @@ -155,14 +150,14 @@ class TestDiffDriveController : public ::testing::Test std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; - hardware_interface::StateInterface left_wheel_pos_state_{left_wheel_names[0], HW_IF_POSITION, - &position_values_[0]}; - hardware_interface::StateInterface right_wheel_pos_state_{right_wheel_names[0], HW_IF_POSITION, - &position_values_[1]}; - hardware_interface::CommandInterface left_wheel_vel_cmd_{left_wheel_names[0], HW_IF_VELOCITY, - &velocity_values_[0]}; - hardware_interface::CommandInterface right_wheel_vel_cmd_{right_wheel_names[0], HW_IF_VELOCITY, - &velocity_values_[1]}; + hardware_interface::StateInterface left_wheel_pos_state_{ + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; + hardware_interface::StateInterface right_wheel_pos_state_{ + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + hardware_interface::CommandInterface left_wheel_vel_cmd_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::CommandInterface right_wheel_vel_cmd_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -182,24 +177,16 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(std::vector()))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(std::vector()))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -210,16 +197,12 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); auto extended_right_wheel_names = right_wheel_names; extended_right_wheel_names.push_back("extra_wheel"); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(extended_right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -230,13 +213,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -254,52 +233,38 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } - TEST_F(TestDiffDriveController, activate_succeeds_with_resources_assigned) { const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResources(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } - TEST_F(TestDiffDriveController, cleanup) { const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); @@ -342,13 +307,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "left_wheel_names", - rclcpp::ParameterValue(left_wheel_names))); + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( - rclcpp::Parameter( - "right_wheel_names", - rclcpp::ParameterValue(right_wheel_names))); + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index d9c0812e72..909b68d3cf 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -26,13 +26,13 @@ TEST(TestLoadDiffDriveController, load_controller) std::shared_ptr executor = std::make_shared(); - controller_manager::ControllerManager cm(std::make_unique( - diff_drive_controller_testing::diffbot_urdf), executor, "test_controller_manager"); + controller_manager::ControllerManager cm( + std::make_unique( + diff_drive_controller_testing::diffbot_urdf), + executor, "test_controller_manager"); ASSERT_NO_THROW( - cm.load_controller( - "test_diff_drive_controller", - "diff_drive_controller/DiffDriveController")); + cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController")); rclcpp::shutdown(); } diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index ac95d8567c..d290801917 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -48,12 +48,9 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_joint_group_effort_controller test/test_load_joint_group_effort_controller.cpp diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 6e1b14daf2..903ab1dabd 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -17,12 +17,11 @@ #include -#include "forward_command_controller/forward_command_controller.hpp" #include "effort_controllers/visibility_control.h" +#include "forward_command_controller/forward_command_controller.hpp" namespace effort_controllers { - /** * \brief Forward command controller for a set of effort controlled joints (linear or angular). * @@ -40,12 +39,10 @@ class JointGroupEffortController : public forward_command_controller::ForwardCom JointGroupEffortController(); EFFORT_CONTROLLERS_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; EFFORT_CONTROLLERS_PUBLIC - CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; }; } // namespace effort_controllers diff --git a/effort_controllers/include/effort_controllers/visibility_control.h b/effort_controllers/include/effort_controllers/visibility_control.h index 31ea69cfbe..e3ca28355d 100644 --- a/effort_controllers/include/effort_controllers/visibility_control.h +++ b/effort_controllers/include/effort_controllers/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define EFFORT_CONTROLLERS_EXPORT __attribute__ ((dllexport)) - #define EFFORT_CONTROLLERS_IMPORT __attribute__ ((dllimport)) - #else - #define EFFORT_CONTROLLERS_EXPORT __declspec(dllexport) - #define EFFORT_CONTROLLERS_IMPORT __declspec(dllimport) - #endif - #ifdef EFFORT_CONTROLLERS_BUILDING_DLL - #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT - #else - #define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT - #endif - #define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC - #define EFFORT_CONTROLLERS_LOCAL +#ifdef __GNUC__ +#define EFFORT_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define EFFORT_CONTROLLERS_IMPORT __attribute__((dllimport)) #else - #define EFFORT_CONTROLLERS_EXPORT __attribute__ ((visibility("default"))) - #define EFFORT_CONTROLLERS_IMPORT - #if __GNUC__ >= 4 - #define EFFORT_CONTROLLERS_PUBLIC __attribute__ ((visibility("default"))) - #define EFFORT_CONTROLLERS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define EFFORT_CONTROLLERS_PUBLIC - #define EFFORT_CONTROLLERS_LOCAL - #endif - #define EFFORT_CONTROLLERS_PUBLIC_TYPE +#define EFFORT_CONTROLLERS_EXPORT __declspec(dllexport) +#define EFFORT_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef EFFORT_CONTROLLERS_BUILDING_DLL +#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT +#else +#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT +#endif +#define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC +#define EFFORT_CONTROLLERS_LOCAL +#else +#define EFFORT_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define EFFORT_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define EFFORT_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define EFFORT_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define EFFORT_CONTROLLERS_PUBLIC +#define EFFORT_CONTROLLERS_LOCAL +#endif +#define EFFORT_CONTROLLERS_PUBLIC_TYPE #endif #endif // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index f5f18c7dbb..e9ca03a1f3 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -16,8 +16,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager ros2_control_test_assets diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 81f45de430..82ea21892a 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -14,8 +14,8 @@ #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "effort_controllers/joint_group_effort_controller.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" @@ -30,21 +30,24 @@ JointGroupEffortController::JointGroupEffortController() interface_name_ = hardware_interface::HW_IF_EFFORT; } -controller_interface::return_type -JointGroupEffortController::init( +controller_interface::return_type JointGroupEffortController::init( const std::string & controller_name) { auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupEffortController constructor. get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT)); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -58,7 +61,8 @@ CallbackReturn JointGroupEffortController::on_deactivate( auto ret = ForwardCommandController::on_deactivate(previous_state); // stop all joints - for (auto & command_interface : command_interfaces_) { + for (auto & command_interface : command_interfaces_) + { command_interface.set_value(0.0); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index b27367bfd8..cc16573f5f 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -39,27 +39,18 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription const auto timeout = std::chrono::seconds(10); return wait_set.wait(timeout).kind(); } -} +} // namespace -void JointGroupEffortControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void JointGroupEffortControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } void JointGroupEffortControllerTest::SetUp() { controller_ = std::make_unique(); } -void JointGroupEffortControllerTest::TearDown() -{ - controller_.reset(nullptr); -} +void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { @@ -131,8 +122,7 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -152,8 +142,7 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -200,8 +189,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string( - controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); std_msgs::msg::Float64MultiArray command_msg; command_msg.data = {10.0, 20.0, 30.0}; command_pub->publish(command_msg); @@ -221,7 +209,6 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } - TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) { SetUpController(); diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 96a6c83ea6..6ae9db4670 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -21,12 +21,12 @@ #include "gmock/gmock.h" +#include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "effort_controllers/joint_group_effort_controller.hpp" -using hardware_interface::HW_IF_EFFORT; using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_EFFORT; // subclassing and friending so we can access member variables class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController { diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index d7968f10fe..a216d46db3 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -29,12 +29,11 @@ TEST(TestLoadJointGroupVelocityController, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_group_effort_controller", - "effort_controllers/JointGroupEffortController")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController")); rclcpp::shutdown(); } diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 2092205bdb..0439c97a7d 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -26,10 +26,10 @@ #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "semantic_components/force_torque_sensor.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" +#include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster { diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index ad451d1fc0..cbc19cfa3a 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -23,20 +23,22 @@ namespace force_torque_sensor_broadcaster { - ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() : controller_interface::ControllerInterface() -{} +{ +} -controller_interface::return_type -ForceTorqueSensorBroadcaster::init(const std::string & controller_name) +controller_interface::return_type ForceTorqueSensorBroadcaster::init( + const std::string & controller_name) { auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { node_->declare_parameter("sensor_name", ""); node_->declare_parameter("interface_names.force.x", ""); node_->declare_parameter("interface_names.force.y", ""); @@ -45,7 +47,9 @@ ForceTorqueSensorBroadcaster::init(const std::string & controller_name) node_->declare_parameter("interface_names.torque.y", ""); node_->declare_parameter("interface_names.torque.z", ""); node_->declare_parameter("frame_id", ""); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -64,46 +68,56 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string(); interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string(); - const bool no_interface_names_defined = std::count( - interface_names_.begin(), - interface_names_.end(), "") == 6; + const bool no_interface_names_defined = + std::count(interface_names_.begin(), interface_names_.end(), "") == 6; - if (sensor_name_.empty() && no_interface_names_defined) { + if (sensor_name_.empty() && no_interface_names_defined) + { RCLCPP_ERROR( - node_->get_logger(), "'sensor_name' or at least one " + node_->get_logger(), + "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); return CallbackReturn::ERROR; } - if (!sensor_name_.empty() && !no_interface_names_defined) { + if (!sensor_name_.empty() && !no_interface_names_defined) + { RCLCPP_ERROR( - node_->get_logger(), "both 'sensor_name' and " + node_->get_logger(), + "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); return CallbackReturn::ERROR; } frame_id_ = node_->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) { + if (frame_id_.empty()) + { RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } - if (!sensor_name_.empty()) { + if (!sensor_name_.empty()) + { force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor(sensor_name_)); - } else { + } + else + { force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor( - interface_names_[0], interface_names_[1], interface_names_[2], - interface_names_[3], interface_names_[4], interface_names_[5])); + interface_names_[0], interface_names_[1], interface_names_[2], interface_names_[3], + interface_names_[4], interface_names_[5])); } - try { + try + { // register ft sensor data publisher sensor_state_publisher_ = node_->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -151,7 +165,8 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( controller_interface::return_type ForceTorqueSensorBroadcaster::update() { - if (realtime_publisher_ && realtime_publisher_->trylock()) { + if (realtime_publisher_ && realtime_publisher_->trylock()) + { realtime_publisher_->msg_.header.stamp = node_->now(); force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); realtime_publisher_->unlockAndPublish(); 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 3ad08e4aa3..3d61eb5490 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 @@ -23,10 +23,10 @@ #include #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -50,15 +50,9 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription } // namespace -void ForceTorqueSensorBroadcasterTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void ForceTorqueSensorBroadcasterTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void ForceTorqueSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } void ForceTorqueSensorBroadcasterTest::SetUp() { @@ -66,10 +60,7 @@ void ForceTorqueSensorBroadcasterTest::SetUp() fts_broadcaster_ = std::make_unique(); } -void ForceTorqueSensorBroadcasterTest::TearDown() -{ - fts_broadcaster_.reset(nullptr); -} +void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); } void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { @@ -92,13 +83,9 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) - { - }; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_force_torque_sensor_broadcaster/wrench", - 10, - subs_callback); + "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); // call update to publish the test value ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); @@ -111,7 +98,6 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); } - TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) { SetUpFTSBroadcaster(); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 8104b7e85c..5477b3fa6f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -27,8 +27,8 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables -class FriendForceTorqueSensorBroadcaster : public force_torque_sensor_broadcaster:: - ForceTorqueSensorBroadcaster +class FriendForceTorqueSensorBroadcaster +: public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster { FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterNotSet); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNamesParameterNotSet); diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index e43f70f440..6e9b6b1167 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -35,12 +35,12 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster")); + ASSERT_NO_THROW(cm.load_controller( + "test_force_torque_sensor_broadcaster", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster")); rclcpp::shutdown(); } diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 7021d4294b..4f08cfd2f8 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -57,13 +57,10 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_forward_command_controller test/test_load_forward_command_controller.cpp diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index b550b0fb92..1521fce0a2 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -21,9 +21,9 @@ #include "controller_interface/controller_interface.hpp" #include "forward_command_controller/visibility_control.h" +#include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "rclcpp/subscription.hpp" #include "realtime_tools/realtime_buffer.h" #include "std_msgs/msg/float64_multi_array.hpp" diff --git a/forward_command_controller/include/forward_command_controller/visibility_control.h b/forward_command_controller/include/forward_command_controller/visibility_control.h index 56ae567985..25a0697fdc 100644 --- a/forward_command_controller/include/forward_command_controller/visibility_control.h +++ b/forward_command_controller/include/forward_command_controller/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__ ((dllexport)) - #define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__ ((dllimport)) - #else - #define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) - #define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) - #endif - #ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL - #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT - #else - #define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT - #endif - #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC - #define FORWARD_COMMAND_CONTROLLER_LOCAL +#ifdef __GNUC__ +#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) #else - #define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) - #define FORWARD_COMMAND_CONTROLLER_IMPORT - #if __GNUC__ >= 4 - #define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) - #define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define FORWARD_COMMAND_CONTROLLER_PUBLIC - #define FORWARD_COMMAND_CONTROLLER_LOCAL - #endif - #define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE +#define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL +#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT +#else +#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT +#endif +#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC +#define FORWARD_COMMAND_CONTROLLER_LOCAL +#else +#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define FORWARD_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define FORWARD_COMMAND_CONTROLLER_PUBLIC +#define FORWARD_COMMAND_CONTROLLER_LOCAL +#endif +#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE #endif #endif // FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 304aee85ac..9c0294516c 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -20,8 +20,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager ros2_control_test_assets diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 2ce5c96d97..c4bf8fcccb 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -19,8 +19,8 @@ #include #include -#include "rclcpp/qos.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -32,22 +32,27 @@ ForwardCommandController::ForwardCommandController() : controller_interface::ControllerInterface(), rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) -{} +{ +} -controller_interface::return_type -ForwardCommandController::init(const std::string & controller_name) +controller_interface::return_type ForwardCommandController::init( + const std::string & controller_name) { auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { auto node = get_node(); node->declare_parameter>("joints", std::vector()); node->declare_parameter("interface_name", ""); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -60,27 +65,27 @@ CallbackReturn ForwardCommandController::on_configure( { joint_names_ = node_->get_parameter("joints").as_string_array(); - if (joint_names_.empty()) { + if (joint_names_.empty()) + { RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); return CallbackReturn::ERROR; } // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) { + if (interface_name_.empty()) + { interface_name_ = node_->get_parameter("interface_name").as_string(); } - if (interface_name_.empty()) { + if (interface_name_.empty()) + { RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); return CallbackReturn::ERROR; } joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) - { - rt_command_ptr_.writeFromNonRT(msg); - }); + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -92,7 +97,8 @@ ForwardCommandController::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint : joint_names_) { + for (const auto & joint : joint_names_) + { command_interfaces_config.names.push_back(joint + "/" + interface_name_); } @@ -108,14 +114,17 @@ ForwardCommandController::state_interface_configuration() const // Fill ordered_interfaces with references to the matching interfaces // in the same order as in joint_names -template +template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & joint_names, const std::string & interface_type, std::vector> & ordered_interfaces) { - for (const auto & joint_name : joint_names) { - for (auto & command_interface : unordered_interfaces) { - if ((command_interface.get_name() == joint_name) && + for (const auto & joint_name : joint_names) + { + for (auto & command_interface : unordered_interfaces) + { + if ( + (command_interface.get_name() == joint_name) && (command_interface.get_interface_name() == interface_type)) { ordered_interfaces.push_back(std::ref(command_interface)); @@ -132,14 +141,14 @@ CallbackReturn ForwardCommandController::on_activate( // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" parameter std::vector> ordered_interfaces; - if (!get_ordered_interfaces( - command_interfaces_, joint_names_, interface_name_, - ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) + if ( + !get_ordered_interfaces( + command_interfaces_, joint_names_, interface_name_, ordered_interfaces) || + command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), - "Expected %zu position command interfaces, got %zu", - joint_names_.size(), ordered_interfaces.size()); + node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), + ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -157,20 +166,22 @@ controller_interface::return_type ForwardCommandController::update() auto joint_commands = rt_command_ptr_.readFromRT(); // no command received yet - if (!joint_commands || !(*joint_commands)) { + if (!joint_commands || !(*joint_commands)) + { return controller_interface::return_type::OK; } - if ((*joint_commands)->data.size() != command_interfaces_.size()) { + if ((*joint_commands)->data.size() != command_interfaces_.size()) + { RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), - *node_->get_clock(), 1000, + get_node()->get_logger(), *node_->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; } - for (auto index = 0ul; index < command_interfaces_.size(); ++index) { + for (auto index = 0ul; index < command_interfaces_.size(); ++index) + { command_interfaces_[index].set_value((*joint_commands)->data[index]); } diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index cbc28f67fb..e42fe6e61a 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -22,12 +22,12 @@ #include "gmock/gmock.h" +#include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "forward_command_controller/forward_command_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/qos.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/wait_set.hpp" @@ -46,17 +46,11 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription const auto timeout = std::chrono::seconds(10); return wait_set.wait(timeout).kind(); } -} +} // namespace -void ForwardCommandControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void ForwardCommandControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void ForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } void ForwardCommandControllerTest::SetUp() { @@ -64,10 +58,7 @@ void ForwardCommandControllerTest::SetUp() controller_ = std::make_unique(); } -void ForwardCommandControllerTest::TearDown() -{ - controller_.reset(nullptr); -} +void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { @@ -195,8 +186,7 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -220,8 +210,7 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -273,8 +262,7 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string( - controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); std_msgs::msg::Float64MultiArray command_msg; command_msg.data = {10.0, 20.0, 30.0}; command_pub->publish(command_msg); diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 5b816bca52..5a5ef32f48 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -25,8 +25,8 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -using hardware_interface::HW_IF_POSITION; using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables class FriendForwardCommandController : public forward_command_controller::ForwardCommandController diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index fe4d0835ef..464b57b69d 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -31,12 +31,11 @@ TEST(TestLoadForwardCommandController, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_forward_command_controller", - "forward_command_controller/ForwardCommandController")); + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); rclcpp::shutdown(); } diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 9ccb4a7913..0414a9104c 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -61,12 +61,9 @@ install(TARGETS gripper_action_controller if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_gripper_action_controllers test/test_load_gripper_action_controllers.cpp diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index d6078cb590..cbb9d216c9 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -47,7 +47,6 @@ namespace gripper_action_controller { - /** * \brief Controller for executing a gripper command action for simple * single-dof grippers. @@ -56,9 +55,8 @@ namespace gripper_action_controller * hardware_interface::HW_IF_POSITION and \p * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. */ -template -class GripperActionController - : public controller_interface::ControllerInterface +template +class GripperActionController : public controller_interface::ControllerInterface { public: /** @@ -74,39 +72,36 @@ class GripperActionController GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; /** * @brief command_interface_configuration This controller requires the * position command interfaces for the controlled joints */ GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** * @brief command_interface_configuration This controller requires the * position and velocity state interfaces for the controlled joints */ GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update() override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; // pre-allocated memory that is re-used to set the realtime buffer @@ -117,8 +112,8 @@ class GripperActionController using ActionServer = rclcpp_action::Server; using ActionServerPtr = ActionServer::SharedPtr; using GoalHandle = rclcpp_action::ServerGoalHandle; - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle< - control_msgs::action::GripperCommand>; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using HwIfaceAdapter = HardwareInterfaceAdapter; @@ -128,19 +123,17 @@ class GripperActionController bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. std::experimental::optional> - joint_position_command_interface_; + joint_position_command_interface_; std::experimental::optional> - joint_position_state_interface_; + joint_position_state_interface_; std::experimental::optional> - joint_velocity_state_interface_; + joint_velocity_state_interface_; std::string joint_name_; ///< Controlled joint names. - HwIfaceAdapter - hw_iface_adapter_; ///< Adapts desired goal state to HW interface. + HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - RealtimeGoalHandlePtr - rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; @@ -150,13 +143,10 @@ class GripperActionController rclcpp::TimerBase::SharedPtr goal_handle_timer_; - rclcpp_action::GoalResponse - goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse - cancel_callback(const std::shared_ptr goal_handle); + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); void accepted_callback(std::shared_ptr goal_handle); @@ -164,21 +154,20 @@ class GripperActionController void set_hold_position(); - rclcpp::Time last_movement_time_ = - rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time - double computed_command_; ///< Computed command + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command double stall_timeout_, - stall_velocity_threshold_; ///< Stall related parameters - double default_max_effort_; ///< Max allowed effort + stall_velocity_threshold_; ///< Stall related parameters + double default_max_effort_; ///< Max allowed effort double goal_tolerance_; /** * \brief Check for success and publish appropriate result and feedback. **/ void check_for_success( - const rclcpp::Time & time, double error_position, - double current_position, double current_velocity); + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); }; } // namespace gripper_action_controller diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index b61eca7d38..96cf9465b9 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -22,27 +22,26 @@ namespace gripper_action_controller { - -template +template void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - if (rt_active_goal_) { + if (rt_active_goal_) + { // Marks the current goal as canceled - rt_active_goal_->setCanceled( - std::make_shared()); + rt_active_goal_->setCanceled(std::make_shared()); rt_active_goal_.reset(); } } -template -controller_interface::return_type -GripperActionController::init( +template +controller_interface::return_type GripperActionController::init( const std::string & controller_name) { // initialize lifecycle node const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } @@ -57,9 +56,8 @@ GripperActionController::init( return controller_interface::return_type::OK; } -template -controller_interface::return_type -GripperActionController::update() +template +controller_interface::return_type GripperActionController::update() { command_struct_rt_ = *(command_.readFromRT()); @@ -69,9 +67,7 @@ GripperActionController::update() const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; - check_for_success( - node_->now(), error_position, current_position, - current_velocity); + check_for_success(node_->now(), error_position, current_position, current_velocity); // Hardware interface adapter: Generate and send commands computed_command_ = hw_iface_adapter_.updateCommand( @@ -80,19 +76,17 @@ GripperActionController::update() return controller_interface::return_type::OK; } -template -rclcpp_action::GoalResponse -GripperActionController::goal_callback( - const rclcpp_action::GoalUUID &, - std::shared_ptr) +template +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr) { RCLCPP_INFO(node_->get_logger(), "Received & accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -template +template void GripperActionController::accepted_callback( - std::shared_ptr goal_handle) // Try to update goal + std::shared_ptr goal_handle) // Try to update goal { { auto rt_goal = std::make_shared(goal_handle); @@ -119,21 +113,20 @@ void GripperActionController::accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } -template -rclcpp_action::CancelResponse -GripperActionController::cancel_callback( +template +rclcpp_action::CancelResponse GripperActionController::cancel_callback( const std::shared_ptr goal_handle) { RCLCPP_INFO(node_->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) { + if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) + { // Enter hold current position mode set_hold_position(); RCLCPP_INFO( - node_->get_logger(), - "Canceling active action goal because cancel callback received."); + node_->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -144,7 +137,7 @@ GripperActionController::cancel_callback( return rclcpp_action::CancelResponse::ACCEPT; } -template +template void GripperActionController::set_hold_position() { command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -152,26 +145,33 @@ void GripperActionController::set_hold_position() command_.writeFromNonRT(command_struct_); } -template +template void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - if (!rt_active_goal_) { + if (!rt_active_goal_) + { return; } - if (fabs(error_position) < goal_tolerance_) { + if (fabs(error_position) < goal_tolerance_) + { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; rt_active_goal_->setSucceeded(pre_alloc_result_); rt_active_goal_.reset(); - } else { - if (fabs(current_velocity) > stall_velocity_threshold_) { + } + else + { + if (fabs(current_velocity) > stall_velocity_threshold_) + { last_movement_time_ = time; - } else if ((time - last_movement_time_).seconds() > stall_timeout_) { + } + else if ((time - last_movement_time_).seconds() > stall_timeout_) + { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; @@ -182,28 +182,25 @@ void GripperActionController::check_for_success( } } -template +template rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_configure( - const rclcpp_lifecycle::State &) +GripperActionController::on_configure(const rclcpp_lifecycle::State &) { const auto logger = node_->get_logger(); // Action status checking update rate - const auto action_monitor_rate = - node_->get_parameter("action_monitor_rate").as_double(); - action_monitor_period_ = rclcpp::Duration::from_seconds( - 1.0 / node_->get_parameter("action_monitor_rate").as_double()); + const auto action_monitor_rate = node_->get_parameter("action_monitor_rate").as_double(); + action_monitor_period_ = + rclcpp::Duration::from_seconds(1.0 / node_->get_parameter("action_monitor_rate").as_double()); RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << - action_monitor_rate << "Hz."); + logger, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); // Controlled joint joint_name_ = node_->get_parameter("joint").as_string(); - if (joint_name_.empty()) { + if (joint_name_.empty()) + { RCLCPP_ERROR(logger, "Could not find joint name on param server"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } // Default tolerances @@ -213,77 +210,68 @@ GripperActionController::on_configure( default_max_effort_ = node_->get_parameter("max_effort").as_double(); default_max_effort_ = fabs(default_max_effort_); // Stall - stall velocity threshold, stall timeout - stall_velocity_threshold_ = - node_->get_parameter("stall_velocity_threshold").as_double(); + stall_velocity_threshold_ = node_->get_parameter("stall_velocity_threshold").as_double(); stall_timeout_ = node_->get_parameter("stall_timeout").as_double(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -template +template rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_activate( - const rclcpp_lifecycle::State &) +GripperActionController::on_activate(const rclcpp_lifecycle::State &) { auto position_command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [](const hardware_interface::LoanedCommandInterface & command_interface) { - return command_interface.get_interface_name() == - hardware_interface::HW_IF_POSITION; + return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_command_interface_it == command_interfaces_.end()) { + if (position_command_interface_it == command_interfaces_.end()) + { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position command interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_name() != joint_name_) { + if (position_command_interface_it->get_name() != joint_name_) + { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Position command interface is different than joint name `" << - position_command_interface_it->get_name() << "` != `" << - joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + node_->get_logger(), "Position command interface is different than joint name `" + << position_command_interface_it->get_name() << "` != `" << joint_name_ + << "`"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == - hardware_interface::HW_IF_POSITION; + return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_state_interface_it == state_interfaces_.end()) { + if (position_state_interface_it == state_interfaces_.end()) + { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position state interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - if (position_state_interface_it->get_name() != joint_name_) { + if (position_state_interface_it->get_name() != joint_name_) + { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Position state interface is different than joint name `" << - position_state_interface_it->get_name() << "` != `" << - joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + node_->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_name() << "` != `" << joint_name_ + << "`"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == - hardware_interface::HW_IF_VELOCITY; + return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); - if (velocity_state_interface_it == state_interfaces_.end()) { + if (velocity_state_interface_it == state_interfaces_.end()) + { RCLCPP_ERROR(node_->get_logger(), "Expected 1 velocity state interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - if (velocity_state_interface_it->get_name() != joint_name_) { + if (velocity_state_interface_it->get_name() != joint_name_) + { RCLCPP_ERROR_STREAM( - node_->get_logger(), - "Velocity command interface is different than joint name `" << - velocity_state_interface_it->get_name() << "` != `" << - joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::ERROR; + node_->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_name() << "` != `" << joint_name_ + << "`"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } joint_position_command_interface_ = *position_command_interface_it; @@ -298,61 +286,57 @@ GripperActionController::on_activate( command_struct_.max_effort_ = default_max_effort_; // Result - pre_alloc_result_ = - std::make_shared(); + pre_alloc_result_ = std::make_shared(); pre_alloc_result_->position = command_struct_.position_; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; // Action interface - action_server_ = - rclcpp_action::create_server( + action_server_ = rclcpp_action::create_server( node_, "~/gripper_cmd", std::bind( - &GripperActionController::goal_callback, this, std::placeholders::_1, - std::placeholders::_2), + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -template +template rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_deactivate( - const rclcpp_lifecycle::State &) +GripperActionController::on_deactivate(const rclcpp_lifecycle::State &) { joint_position_command_interface_ = std::experimental::nullopt; joint_position_state_interface_ = std::experimental::nullopt; joint_velocity_state_interface_ = std::experimental::nullopt; release_interfaces(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: - CallbackReturn::SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -template +template controller_interface::InterfaceConfiguration -GripperActionController::command_interface_configuration() -const +GripperActionController::command_interface_configuration() const { - return {controller_interface::interface_configuration_type::INDIVIDUAL, + return { + controller_interface::interface_configuration_type::INDIVIDUAL, {joint_name_ + "/" + hardware_interface::HW_IF_POSITION}}; } -template +template controller_interface::InterfaceConfiguration -GripperActionController::state_interface_configuration() -const +GripperActionController::state_interface_configuration() const { - return {controller_interface::interface_configuration_type::INDIVIDUAL, + return { + controller_interface::interface_configuration_type::INDIVIDUAL, {joint_name_ + "/" + hardware_interface::HW_IF_POSITION, - joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}}; + joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}}; } -template +template GripperActionController::GripperActionController() -: controller_interface::ControllerInterface(), action_monitor_period_(0) {} +: controller_interface::ControllerInterface(), action_monitor_period_(0) +{ +} } // namespace gripper_action_controller diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 9926281aea..1e1a6a2c8c 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -39,13 +39,13 @@ * it is supposed to work with either position or effort commands. * */ -template +template class HardwareInterfaceAdapter { public: bool init( std::experimental::optional< - std::reference_wrapper>/* joint_handle */, + std::reference_wrapper> /* joint_handle */, const rclcpp::Node::SharedPtr & /* node */) { return false; @@ -55,9 +55,8 @@ class HardwareInterfaceAdapter void stopping(const rclcpp::Time & /* time */) {} double updateCommand( - double /* desired_position */, double /* desired_velocity */, - double /* error_position */, double /* error_velocity */, - double /* max_allowed_effort */) + double /* desired_position */, double /* desired_velocity */, double /* error_position */, + double /* error_velocity */, double /* max_allowed_effort */) { return 0.0; } @@ -67,13 +66,13 @@ class HardwareInterfaceAdapter * \brief Adapter for a position-controlled hardware interface. Forwards desired * positions as commands. */ -template<> +template <> class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional< - std::reference_wrapper> joint_handle, + std::experimental::optional> + joint_handle, const rclcpp::Node::SharedPtr & /* node */) { joint_handle_ = joint_handle; @@ -84,9 +83,8 @@ class HardwareInterfaceAdapter void stopping(const rclcpp::Time & /* time */) {} double updateCommand( - double desired_position, double /* desired_velocity */, - double /* error_position */, double /* error_velocity */, - double max_allowed_effort) + double desired_position, double /* desired_velocity */, double /* error_position */, + double /* error_velocity */, double max_allowed_effort) { // Forward desired position to command joint_handle_->get().set_value(desired_position); @@ -95,7 +93,7 @@ class HardwareInterfaceAdapter private: std::experimental::optional> - joint_handle_; + joint_handle_; }; /** @@ -112,13 +110,13 @@ class HardwareInterfaceAdapter * gripper_joint: {p: 200, d: 1, i: 5, i_clamp: 1} * \endcode */ -template<> +template <> class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional< - std::reference_wrapper> joint_handle, + std::experimental::optional> + joint_handle, const rclcpp::Node::SharedPtr & node) { joint_handle_ = joint_handle; @@ -127,18 +125,16 @@ class HardwareInterfaceAdapter const auto k_p = node->declare_parameter(prefix + ".p", 0.0); const auto k_i = node->declare_parameter(prefix + ".i", 0.0); const auto k_d = node->declare_parameter(prefix + ".d", 0.0); - const auto i_clamp = - node->declare_parameter(prefix + ".i_clamp", 0.0); + const auto i_clamp = node->declare_parameter(prefix + ".i_clamp", 0.0); // Initialize PID - pid_ = std::make_shared( - k_p, k_i, k_d, i_clamp, - -i_clamp); + pid_ = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); return true; } void starting(const rclcpp::Time & /* time */) { - if (!joint_handle_) { + if (!joint_handle_) + { return; } // Reset PIDs, zero effort commands @@ -149,23 +145,20 @@ class HardwareInterfaceAdapter void stopping(const rclcpp::Time & /* time */) {} double updateCommand( - double /* desired_position */, double /* desired_velocity */, - double error_position, double error_velocity, - double max_allowed_effort) + double /* desired_position */, double /* desired_velocity */, double error_position, + double error_velocity, double max_allowed_effort) { // Preconditions - if (!joint_handle_) { + if (!joint_handle_) + { return 0.0; } // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = - pid_->computeCommand(error_position, error_velocity, period.count()); - command = - std::min( - fabs(max_allowed_effort), - std::max(-fabs(max_allowed_effort), command)); + double command = pid_->computeCommand(error_position, error_velocity, period.count()); + command = std::min( + fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); last_update_time_ = std::chrono::steady_clock::now(); return command; @@ -175,7 +168,7 @@ class HardwareInterfaceAdapter using PidPtr = std::shared_ptr; PidPtr pid_; std::experimental::optional> - joint_handle_; + joint_handle_; std::chrono::steady_clock::time_point last_update_time_; }; diff --git a/gripper_controllers/include/gripper_controllers/visibility_control.hpp b/gripper_controllers/include/gripper_controllers/visibility_control.hpp index 9081e09db3..76b8dafe54 100644 --- a/gripper_controllers/include/gripper_controllers/visibility_control.hpp +++ b/gripper_controllers/include/gripper_controllers/visibility_control.hpp @@ -27,28 +27,28 @@ #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ - #define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__ ((dllexport)) - #define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__ ((dllimport)) - #else - #define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) - #define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) - #endif - #ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL - #define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT - #else - #define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT - #endif - #define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC - #define GRIPPER_ACTION_CONTROLLER_LOCAL +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) #else -#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) #define GRIPPER_ACTION_CONTROLLER_IMPORT #if __GNUC__ >= 4 -#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) -#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) #else #define GRIPPER_ACTION_CONTROLLER_PUBLIC - #define GRIPPER_ACTION_CONTROLLER_LOCAL +#define GRIPPER_ACTION_CONTROLLER_LOCAL #endif #define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE #endif diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 4ade7a9d6d..da568d0f45 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -25,8 +25,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager ros2_control_test_assets diff --git a/gripper_controllers/src/gripper_action_controller.cpp b/gripper_controllers/src/gripper_action_controller.cpp index b0621bbf09..a41e41000a 100644 --- a/gripper_controllers/src/gripper_action_controller.cpp +++ b/gripper_controllers/src/gripper_action_controller.cpp @@ -24,8 +24,7 @@ namespace position_controllers * commands to a \b position interface. */ using GripperActionController = - gripper_action_controller::GripperActionController< - hardware_interface::HW_IF_POSITION>; + gripper_action_controller::GripperActionController; } // namespace position_controllers namespace effort_controllers @@ -35,15 +34,12 @@ namespace effort_controllers * commands to a \b effort interface. */ using GripperActionController = - gripper_action_controller::GripperActionController< - hardware_interface::HW_IF_EFFORT>; + gripper_action_controller::GripperActionController; } // namespace effort_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - position_controllers::GripperActionController, - controller_interface::ControllerInterface) + position_controllers::GripperActionController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( - effort_controllers::GripperActionController, - controller_interface::ControllerInterface) + effort_controllers::GripperActionController, controller_interface::ControllerInterface) diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 59cb60cdc9..b355cdf34a 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -29,16 +29,13 @@ TEST(TestLoadGripperActionControllers, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_gripper_action_position_controller", - "position_controllers/GripperActionController")); - ASSERT_NO_THROW( - cm.load_controller( - "test_gripper_action_effort_controller", - "effort_controllers/GripperActionController")); + ASSERT_NO_THROW(cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController")); + ASSERT_NO_THROW(cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController")); rclcpp::shutdown(); } diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index f70fd7fa82..8b8154aafd 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -63,14 +63,11 @@ install( ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index c46473f9a9..39f4f9e11c 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,11 +25,11 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" -#include "semantic_components/imu_sensor.hpp" -#include "sensor_msgs/msg/imu.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" +#include "semantic_components/imu_sensor.hpp" +#include "sensor_msgs/msg/imu.hpp" namespace imu_sensor_broadcaster { diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 5fba0733f4..34cc3b9d76 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -23,19 +23,21 @@ namespace imu_sensor_broadcaster { - -controller_interface::return_type -IMUSensorBroadcaster::init(const std::string & controller_name) +controller_interface::return_type IMUSensorBroadcaster::init(const std::string & controller_name) { auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { node_->declare_parameter("sensor_name", ""); node_->declare_parameter("frame_id", ""); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { RCLCPP_ERROR( node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; @@ -48,26 +50,30 @@ CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { sensor_name_ = node_->get_parameter("sensor_name").as_string(); - if (sensor_name_.empty()) { - RCLCPP_ERROR( - node_->get_logger(), "'sensor_name' parameter has to be specified."); + if (sensor_name_.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } frame_id_ = node_->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) { + if (frame_id_.empty()) + { RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } - imu_sensor_ = std::make_unique( - semantic_components::IMUSensor(sensor_name_)); - try { + imu_sensor_ = + std::make_unique(semantic_components::IMUSensor(sensor_name_)); + try + { // register ft sensor data publisher - sensor_state_publisher_ = node_->create_publisher( - "~/imu", rclcpp::SystemDefaultsQoS()); + sensor_state_publisher_ = + node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -82,16 +88,16 @@ CallbackReturn IMUSensorBroadcaster::on_configure( return CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration -IMUSensorBroadcaster::command_interface_configuration() const +controller_interface::InterfaceConfiguration IMUSensorBroadcaster::command_interface_configuration() + const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; return command_interfaces_config; } -controller_interface::InterfaceConfiguration -IMUSensorBroadcaster::state_interface_configuration() const +controller_interface::InterfaceConfiguration IMUSensorBroadcaster::state_interface_configuration() + const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -99,8 +105,7 @@ IMUSensorBroadcaster::state_interface_configuration() const return state_interfaces_config; } -CallbackReturn IMUSensorBroadcaster::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn IMUSensorBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { imu_sensor_->assign_loaned_state_interfaces(state_interfaces_); return CallbackReturn::SUCCESS; @@ -115,7 +120,8 @@ CallbackReturn IMUSensorBroadcaster::on_deactivate( controller_interface::return_type IMUSensorBroadcaster::update() { - if (realtime_publisher_ && realtime_publisher_->trylock()) { + if (realtime_publisher_ && realtime_publisher_->trylock()) + { realtime_publisher_->msg_.header.stamp = node_->now(); imu_sensor_->get_values_as_message(realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); @@ -129,5 +135,4 @@ controller_interface::return_type IMUSensorBroadcaster::update() #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - imu_sensor_broadcaster::IMUSensorBroadcaster, - controller_interface::ControllerInterface) + imu_sensor_broadcaster::IMUSensorBroadcaster, controller_interface::ControllerInterface) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index e87b24b1f3..3edc555dd7 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -22,10 +22,10 @@ #include #include -#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -49,15 +49,9 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription } // namespace -void IMUSensorBroadcasterTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void IMUSensorBroadcasterTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void IMUSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } void IMUSensorBroadcasterTest::SetUp() { @@ -65,10 +59,7 @@ void IMUSensorBroadcasterTest::SetUp() imu_broadcaster_ = std::make_unique(); } -void IMUSensorBroadcasterTest::TearDown() -{ - imu_broadcaster_.reset(nullptr); -} +void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { @@ -90,18 +81,13 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() imu_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -void IMUSensorBroadcasterTest::subscribe_and_get_message( - sensor_msgs::msg::Imu & imu_msg) +void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) - { - }; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_imu_sensor_broadcaster/imu", - 10, - subs_callback); + "/test_imu_sensor_broadcaster/imu", 10, subs_callback); // call update to publish the test value ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK); @@ -114,7 +100,6 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message( ASSERT_TRUE(subscription->take(imu_msg, msg_info)); } - TEST_F(IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) { SetUpIMUBroadcaster(); @@ -129,11 +114,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) // set the 'interface_names' imu_broadcaster_->get_node()->set_parameter( - {"interface_names.angular_velocity.x", - "imu_sensor/angular_velocity.x"}); + {"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"}); imu_broadcaster_->get_node()->set_parameter( - {"interface_names.linear_acceleration.z", - "imu_sensor/linear_acceleration.z"}); + {"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index ef66496cc5..01423724b8 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -27,8 +27,7 @@ #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables -class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster:: - IMUSensorBroadcaster +class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster::IMUSensorBroadcaster { FRIEND_TEST(IMUSensorBroadcasterTest, SensorNameParameterNotSet); FRIEND_TEST(IMUSensorBroadcasterTest, InterfaceNamesParameterNotSet); @@ -56,27 +55,26 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - hardware_interface::StateInterface imu_orientation_x_{sensor_name_, "orientation.x", - &sensor_values_[0]}; - hardware_interface::StateInterface imu_orientation_y_{sensor_name_, "orientation.y", - &sensor_values_[1]}; - hardware_interface::StateInterface imu_orientation_z_{sensor_name_, "orientation.z", - &sensor_values_[2]}; - hardware_interface::StateInterface imu_orientation_w_{sensor_name_, "orientation.w", - &sensor_values_[3]}; - hardware_interface::StateInterface imu_angular_velocity_x_{sensor_name_, "angular_velocity.x", - &sensor_values_[4]}; - hardware_interface::StateInterface imu_angular_velocity_y_{sensor_name_, "angular_velocity.y", - &sensor_values_[5]}; - hardware_interface::StateInterface imu_angular_velocity_z_{sensor_name_, "angular_velocity.z", - &sensor_values_[6]}; - hardware_interface::StateInterface imu_linear_acceleration_x_{sensor_name_, - "linear_acceleration.x", &sensor_values_[7]}; - hardware_interface::StateInterface imu_linear_acceleration_y_{sensor_name_, - "linear_acceleration.y", &sensor_values_[8]}; - hardware_interface::StateInterface imu_linear_acceleration_z_{sensor_name_, - "linear_acceleration.z", - &sensor_values_[9]}; + hardware_interface::StateInterface imu_orientation_x_{ + sensor_name_, "orientation.x", &sensor_values_[0]}; + hardware_interface::StateInterface imu_orientation_y_{ + sensor_name_, "orientation.y", &sensor_values_[1]}; + hardware_interface::StateInterface imu_orientation_z_{ + sensor_name_, "orientation.z", &sensor_values_[2]}; + hardware_interface::StateInterface imu_orientation_w_{ + sensor_name_, "orientation.w", &sensor_values_[3]}; + hardware_interface::StateInterface imu_angular_velocity_x_{ + sensor_name_, "angular_velocity.x", &sensor_values_[4]}; + hardware_interface::StateInterface imu_angular_velocity_y_{ + sensor_name_, "angular_velocity.y", &sensor_values_[5]}; + hardware_interface::StateInterface imu_angular_velocity_z_{ + sensor_name_, "angular_velocity.z", &sensor_values_[6]}; + hardware_interface::StateInterface imu_linear_acceleration_x_{ + sensor_name_, "linear_acceleration.x", &sensor_values_[7]}; + hardware_interface::StateInterface imu_linear_acceleration_y_{ + sensor_name_, "linear_acceleration.y", &sensor_values_[8]}; + hardware_interface::StateInterface imu_linear_acceleration_z_{ + sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; std::unique_ptr imu_broadcaster_; diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index 55bdb63e38..e90c9915ed 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -35,12 +35,11 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_imu_sensor_broadcaster", - "imu_sensor_broadcaster/IMUSensorBroadcaster")); + ASSERT_NO_THROW(cm.load_controller( + "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster")); rclcpp::shutdown(); } diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 56bc8c0660..a656f4a334 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -54,14 +54,11 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_joint_state_broadcaster test/test_load_joint_state_broadcaster.cpp diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 49ff1734ca..20f33722ec 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -29,7 +29,6 @@ namespace joint_state_broadcaster { - class JointStateBroadcaster : public controller_interface::ControllerInterface { public: @@ -37,8 +36,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface JointStateBroadcaster(); JOINT_STATE_BROADCASTER_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -47,20 +45,19 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_STATE_BROADCASTER_PUBLIC - controller_interface::return_type - update() override; + controller_interface::return_type update() override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; protected: bool init_joint_data(); @@ -73,15 +70,14 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface // For the JointState message, // we store the name of joints with compatible interfaces std::vector joint_names_; - std::shared_ptr> - joint_state_publisher_; + std::shared_ptr> joint_state_publisher_; sensor_msgs::msg::JointState joint_state_msg_; // For the DynamicJointState format, we use a map to buffer values in for easier lookup // This allows to preserve whatever order or names/interfaces were initialized. std::unordered_map> name_if_value_mapping_; std::shared_ptr> - dynamic_joint_state_publisher_; + dynamic_joint_state_publisher_; control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; }; diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h b/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h index 545123cc4e..8bbe0fcd73 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h +++ b/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define JOINT_STATE_BROADCASTER_EXPORT __attribute__ ((dllexport)) - #define JOINT_STATE_BROADCASTER_IMPORT __attribute__ ((dllimport)) - #else - #define JOINT_STATE_BROADCASTER_EXPORT __declspec(dllexport) - #define JOINT_STATE_BROADCASTER_IMPORT __declspec(dllimport) - #endif - #ifdef JOINT_STATE_BROADCASTER_BUILDING_DLL - #define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_EXPORT - #else - #define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_IMPORT - #endif - #define JOINT_STATE_BROADCASTER_PUBLIC_TYPE JOINT_STATE_BROADCASTER_PUBLIC - #define JOINT_STATE_BROADCASTER_LOCAL +#ifdef __GNUC__ +#define JOINT_STATE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define JOINT_STATE_BROADCASTER_IMPORT __attribute__((dllimport)) #else - #define JOINT_STATE_BROADCASTER_EXPORT __attribute__ ((visibility("default"))) - #define JOINT_STATE_BROADCASTER_IMPORT - #if __GNUC__ >= 4 - #define JOINT_STATE_BROADCASTER_PUBLIC __attribute__ ((visibility("default"))) - #define JOINT_STATE_BROADCASTER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define JOINT_STATE_BROADCASTER_PUBLIC - #define JOINT_STATE_BROADCASTER_LOCAL - #endif - #define JOINT_STATE_BROADCASTER_PUBLIC_TYPE +#define JOINT_STATE_BROADCASTER_EXPORT __declspec(dllexport) +#define JOINT_STATE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_STATE_BROADCASTER_BUILDING_DLL +#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_EXPORT +#else +#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_IMPORT +#endif +#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE JOINT_STATE_BROADCASTER_PUBLIC +#define JOINT_STATE_BROADCASTER_LOCAL +#else +#define JOINT_STATE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define JOINT_STATE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define JOINT_STATE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define JOINT_STATE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_STATE_BROADCASTER_PUBLIC +#define JOINT_STATE_BROADCASTER_LOCAL +#endif +#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE #endif #endif // JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index ed6da47519..fdcd281737 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -25,8 +25,6 @@ sensor_msgs ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager rclcpp ros2_control_test_assets diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 9f7f91a7f4..f17df64654 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -40,25 +40,26 @@ class State; namespace joint_state_broadcaster { const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); +using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::HW_IF_EFFORT; +JointStateBroadcaster::JointStateBroadcaster() {} -JointStateBroadcaster::JointStateBroadcaster() -{} - -controller_interface::return_type -JointStateBroadcaster::init(const std::string & controller_name) +controller_interface::return_type JointStateBroadcaster::init(const std::string & controller_name) { auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { get_node()->declare_parameter("use_local_topics", false); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -69,15 +70,15 @@ JointStateBroadcaster::init(const std::string & controller_name) controller_interface::InterfaceConfiguration JointStateBroadcaster::command_interface_configuration() const { - return controller_interface::InterfaceConfiguration{controller_interface:: - interface_configuration_type::NONE}; + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; } controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interface_configuration() -const + const { - return controller_interface::InterfaceConfiguration{controller_interface:: - interface_configuration_type::ALL}; + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::ALL}; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -85,7 +86,8 @@ JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_s { use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); - try { + try + { const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; joint_state_publisher_ = get_node()->create_publisher( @@ -93,8 +95,10 @@ JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_s dynamic_joint_state_publisher_ = get_node()->create_publisher( - topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); - } catch (const std::exception & e) { + topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); + } + catch (const std::exception & e) + { // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -105,7 +109,8 @@ JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_s rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointStateBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - if (!init_joint_data()) { + if (!init_joint_data()) + { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -121,15 +126,16 @@ JointStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -template +template bool has_any_key( - const std::unordered_map & map, - const std::vector & keys) + const std::unordered_map & map, const std::vector & keys) { bool found_key = false; - for (const auto & key_item : map) { + for (const auto & key_item : map) + { const auto & key = key_item.first; - if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) { + if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) + { found_key = true; break; } @@ -140,21 +146,24 @@ bool has_any_key( bool JointStateBroadcaster::init_joint_data() { // loop in reverse order, this maintains the order of values at retrieval time - for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { + for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) + { // initialize map if name is new - if (name_if_value_mapping_.count(si->get_name()) == 0) { + if (name_if_value_mapping_.count(si->get_name()) == 0) + { name_if_value_mapping_[si->get_name()] = {}; } // add interface name - name_if_value_mapping_[si->get_name()][si->get_interface_name()] = - kUninitializedValue; + name_if_value_mapping_[si->get_name()][si->get_interface_name()] = kUninitializedValue; } // filter state interfaces that have at least one of the joint_states fields, // the rest will be ignored for this message - for (const auto & name_ifv : name_if_value_mapping_) { + for (const auto & name_ifv : name_if_value_mapping_) + { const auto & interfaces_and_values = name_ifv.second; - if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { + if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) + { joint_names_.push_back(name_ifv.first); } } @@ -162,12 +171,15 @@ bool JointStateBroadcaster::init_joint_data() // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there rclcpp::Parameter extra_joints; - if (get_node()->get_parameter("extra_joints", extra_joints)) { + if (get_node()->get_parameter("extra_joints", extra_joints)) + { const std::vector & extra_joints_names = extra_joints.as_string_array(); - for (const auto & extra_joint_name : extra_joints_names) { - if (name_if_value_mapping_.count(extra_joint_name) == 0) { - name_if_value_mapping_[extra_joint_name] = - {{HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; + for (const auto & extra_joint_name : extra_joints_names) + { + if (name_if_value_mapping_.count(extra_joint_name) == 0) + { + name_if_value_mapping_[extra_joint_name] = { + {HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; joint_names_.push_back(extra_joint_name); } } @@ -192,12 +204,14 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { - for (const auto & name_ifv : name_if_value_mapping_) { + for (const auto & name_ifv : name_if_value_mapping_) + { const auto & name = name_ifv.first; const auto & interfaces_and_values = name_ifv.second; dynamic_joint_state_msg_.joint_names.push_back(name); control_msgs::msg::InterfaceValue if_value; - for (const auto & interface_and_value : interfaces_and_values) { + for (const auto & interface_and_value : interfaces_and_values) + { if_value.interface_names.emplace_back(interface_and_value.first); if_value.values.emplace_back(kUninitializedValue); } @@ -211,31 +225,33 @@ double get_value( { const auto & interfaces_and_values = map.at(name); const auto interface_and_value = interfaces_and_values.find(interface_name); - if (interface_and_value != interfaces_and_values.cend()) { + if (interface_and_value != interfaces_and_values.cend()) + { return interface_and_value->second; - } else { + } + else + { return kUninitializedValue; } } -controller_interface::return_type -JointStateBroadcaster::update() +controller_interface::return_type JointStateBroadcaster::update() { - for (const auto & state_interface : state_interfaces_) { + for (const auto & state_interface : state_interfaces_) + { name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s/%s: %f\n", - state_interface.get_name().c_str(), - state_interface.get_interface_name().c_str(), - state_interface.get_value()); + get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), + state_interface.get_interface_name().c_str(), state_interface.get_value()); } joint_state_msg_.header.stamp = get_node()->get_clock()->now(); dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now(); // update joint state message and dynamic joint state message - for (auto i = 0ul; i < joint_names_.size(); ++i) { + for (auto i = 0ul; i < joint_names_.size(); ++i) + { joint_state_msg_.position[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION); joint_state_msg_.velocity[i] = @@ -244,13 +260,13 @@ JointStateBroadcaster::update() } for (auto joint_index = 0ul; joint_index < dynamic_joint_state_msg_.joint_names.size(); - ++joint_index) + ++joint_index) { const auto & name = dynamic_joint_state_msg_.joint_names[joint_index]; for (auto interface_index = 0ul; - interface_index < - dynamic_joint_state_msg_.interface_values[joint_index].interface_names.size(); - ++interface_index) + interface_index < + dynamic_joint_state_msg_.interface_values[joint_index].interface_names.size(); + ++interface_index) { const auto & interface_name = dynamic_joint_state_msg_.interface_values[joint_index].interface_names[interface_index]; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 2eabe060a2..5bf3d9dc3d 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -31,16 +31,15 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_state_broadcaster.hpp" +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; using hardware_interface::LoanedStateInterface; using std::placeholders::_1; using testing::Each; using testing::ElementsAreArray; using testing::IsEmpty; using testing::SizeIs; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::HW_IF_EFFORT; - namespace { @@ -58,15 +57,9 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription } } // namespace -void JointStateBroadcasterTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void JointStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void JointStateBroadcasterTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void JointStateBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } void JointStateBroadcasterTest::SetUp() { @@ -74,10 +67,7 @@ void JointStateBroadcasterTest::SetUp() state_broadcaster_ = std::make_unique(); } -void JointStateBroadcasterTest::TearDown() -{ - state_broadcaster_.reset(nullptr); -} +void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } void JointStateBroadcasterTest::SetUpStateBroadcaster() { @@ -171,8 +161,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureSuccessTest) ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_IFS)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, - ElementsAreArray(joint_names_)); + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); ASSERT_THAT( state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); @@ -207,13 +196,9 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) - { - }; - auto subscription = test_node.create_subscription( - topic, - 10, - subs_callback); + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subscription = + test_node.create_subscription(topic, 10, subs_callback); ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); @@ -251,8 +236,8 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) test_published_joint_state_message("joint_state_broadcaster/joint_states"); } -void -JointStateBroadcasterTest::test_published_dynamic_joint_state_message(const std::string & topic) +void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( + const std::string & topic) { auto node_state = state_broadcaster_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -261,13 +246,9 @@ JointStateBroadcasterTest::test_published_dynamic_joint_state_message(const std: ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) - { - }; - auto subscription = test_node.create_subscription( - topic, - 10, - subs_callback); + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subscription = + test_node.create_subscription(topic, 10, subs_callback); ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); @@ -286,16 +267,15 @@ JointStateBroadcasterTest::test_published_dynamic_joint_state_message(const std: // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (auto i = 0ul; i < dynamic_joint_state_msg.joint_names.size(); ++i) { + for (auto i = 0ul; i < dynamic_joint_state_msg.joint_names.size(); ++i) + { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, ElementsAreArray( - INTERFACE_NAMES)); - const auto index_in_original_order = - std::distance( + dynamic_joint_state_msg.interface_values[i].interface_names, + ElementsAreArray(INTERFACE_NAMES)); + const auto index_in_original_order = std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), - dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); ASSERT_THAT( dynamic_joint_state_msg.interface_values[i].values, Each(joint_values_[index_in_original_order])); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index dc04f10486..bffa5294b2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -51,24 +51,24 @@ class JointStateBroadcasterTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_values_ = {1.1, 2.1, 3.1}; - hardware_interface::StateInterface joint_1_pos_state_{joint_names_[0], "position", - &joint_values_[0]}; - hardware_interface::StateInterface joint_2_pos_state_{joint_names_[1], "position", - &joint_values_[1]}; - hardware_interface::StateInterface joint_3_pos_state_{joint_names_[2], "position", - &joint_values_[2]}; - hardware_interface::StateInterface joint_1_vel_state_{joint_names_[0], "velocity", - &joint_values_[0]}; - hardware_interface::StateInterface joint_2_vel_state_{joint_names_[1], "velocity", - &joint_values_[1]}; - hardware_interface::StateInterface joint_3_vel_state_{joint_names_[2], "velocity", - &joint_values_[2]}; - hardware_interface::StateInterface joint_1_eff_state_{joint_names_[0], "effort", - &joint_values_[0]}; - hardware_interface::StateInterface joint_2_eff_state_{joint_names_[1], "effort", - &joint_values_[1]}; - hardware_interface::StateInterface joint_3_eff_state_{joint_names_[2], "effort", - &joint_values_[2]}; + hardware_interface::StateInterface joint_1_pos_state_{ + joint_names_[0], "position", &joint_values_[0]}; + hardware_interface::StateInterface joint_2_pos_state_{ + joint_names_[1], "position", &joint_values_[1]}; + hardware_interface::StateInterface joint_3_pos_state_{ + joint_names_[2], "position", &joint_values_[2]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_names_[0], "velocity", &joint_values_[0]}; + hardware_interface::StateInterface joint_2_vel_state_{ + joint_names_[1], "velocity", &joint_values_[1]}; + hardware_interface::StateInterface joint_3_vel_state_{ + joint_names_[2], "velocity", &joint_values_[2]}; + hardware_interface::StateInterface joint_1_eff_state_{ + joint_names_[0], "effort", &joint_values_[0]}; + hardware_interface::StateInterface joint_2_eff_state_{ + joint_names_[1], "effort", &joint_values_[1]}; + hardware_interface::StateInterface joint_3_eff_state_{ + joint_names_[2], "effort", &joint_values_[2]}; std::unique_ptr state_broadcaster_; }; diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 2e9060e2d9..d7a2934f57 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -29,13 +29,13 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::shared_ptr executor = std::make_shared(); - controller_manager::ControllerManager cm(std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_state_broadcaster", - "joint_state_broadcaster/JointStateBroadcaster")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster")); rclcpp::shutdown(); } diff --git a/joint_state_controller/CMakeLists.txt b/joint_state_controller/CMakeLists.txt index 8596843f13..669460a940 100644 --- a/joint_state_controller/CMakeLists.txt +++ b/joint_state_controller/CMakeLists.txt @@ -38,14 +38,11 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_joint_state_controller test/test_load_joint_state_controller.cpp diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml index 765eae7e79..9d149bfccc 100644 --- a/joint_state_controller/package.xml +++ b/joint_state_controller/package.xml @@ -16,8 +16,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager hardware_interface rclcpp diff --git a/joint_state_controller/test/test_load_joint_state_controller.cpp b/joint_state_controller/test/test_load_joint_state_controller.cpp index 20f71a3979..bfba8a4743 100644 --- a/joint_state_controller/test/test_load_joint_state_controller.cpp +++ b/joint_state_controller/test/test_load_joint_state_controller.cpp @@ -29,15 +29,15 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::shared_ptr executor = std::make_shared(); - controller_manager::ControllerManager cm(std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); // Even though joint_state_controller is deprecated and renamed to joint_state_broadcaster, it // should still be loadable through its old name "joint_state_controller/JointStateController" - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_state_controller", - "joint_state_controller/JointStateController")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_state_controller", "joint_state_controller/JointStateController")); rclcpp::shutdown(); } diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 602b4b2a45..a8a9a0a0a9 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -57,12 +57,9 @@ install(TARGETS joint_trajectory_controller if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_trajectory test/test_trajectory.cpp) target_include_directories(test_trajectory PRIVATE include) target_link_libraries(test_trajectory joint_trajectory_controller) 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 fa7661ddb3..1092dc7287 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 @@ -22,8 +22,8 @@ #include #include -#include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/action/follow_joint_trajectory.hpp" +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/tolerances.hpp" @@ -42,11 +42,11 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -using namespace std::chrono_literals; // NOLINT +using namespace std::chrono_literals; // NOLINT namespace rclcpp_action { -template +template class ServerGoalHandle; } // namespace rclcpp_action namespace rclcpp_lifecycle @@ -65,52 +65,48 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JointTrajectoryController(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; /** * @brief command_interface_configuration This controller requires the position command * interfaces for the controlled joints */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + 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 */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type - update() override; + controller_interface::return_type update() override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_error(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; protected: std::vector joint_names_; @@ -135,7 +131,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ - template + template using InterfaceReferences = std::vector>>; InterfaceReferences joint_command_interface_; @@ -155,15 +151,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr - joint_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr joint_command_subscriber_ = + nullptr; std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; + traj_msg_external_point_ptr_; // The controller should be in halted state after creation otherwise memory corruption // TODO(anyone): Is the variable relevant, since we are using lifecycle? @@ -185,15 +181,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp_action::Server::SharedPtr action_server_; bool allow_partial_joints_goal_ = false; - RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); // callbacks for action_server_ JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp_action::CancelResponse cancel_callback( const std::shared_ptr> goal_handle); @@ -211,17 +206,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void sort_to_local_joint_order( std::shared_ptr trajectory_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_msg( - const trajectory_msgs::msg::JointTrajectory & trajectory) const; + bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void add_new_trajectory_msg( const std::shared_ptr & traj_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( - size_t joint_names_size, - const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, - bool allow_empty) const; + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const; SegmentTolerances default_tolerances_; @@ -236,8 +228,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( - const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); void read_state_from_hardware(JointTrajectoryPoint & state); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index dbba7ca57a..2d247eeb8b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -41,7 +41,6 @@ namespace joint_trajectory_controller { - /** * \brief Trajectory state tolerances for position, velocity and acceleration variables. * @@ -59,10 +58,7 @@ struct StateTolerances */ struct SegmentTolerances { - explicit SegmentTolerances(size_t size = 0) - : state_tolerance(size), - goal_state_tolerance(size) - {} + explicit SegmentTolerances(size_t size = 0) : state_tolerance(size), goal_state_tolerance(size) {} /** State tolerances that apply during segment execution. */ std::vector state_tolerance; @@ -96,8 +92,7 @@ struct SegmentTolerances * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - const rclcpp::Node & node, - const std::vector & joint_names) + const rclcpp::Node & node, const std::vector & joint_names) { const auto n_joints = joint_names.size(); SegmentTolerances tolerances; @@ -105,28 +100,25 @@ SegmentTolerances get_segment_tolerances( // State and goal state tolerances double stopped_velocity_tolerance = 0.01; node.get_parameter_or( - "constraints.stopped_velocity_tolerance", - stopped_velocity_tolerance, stopped_velocity_tolerance); + "constraints.stopped_velocity_tolerance", stopped_velocity_tolerance, + stopped_velocity_tolerance); tolerances.state_tolerance.resize(n_joints); tolerances.goal_state_tolerance.resize(n_joints); - for (auto i = 0ul; i < n_joints; ++i) { + for (auto i = 0ul; i < n_joints; ++i) + { const std::string prefix = "constraints." + joint_names[i]; node.get_parameter_or( - prefix + ".trajectory", tolerances.state_tolerance[i].position, - 0.0); + prefix + ".trajectory", tolerances.state_tolerance[i].position, 0.0); node.get_parameter_or( - prefix + ".goal", tolerances.goal_state_tolerance[i].position, - 0.0); + prefix + ".goal", tolerances.goal_state_tolerance[i].position, 0.0); auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", - (prefix + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (prefix + ".trajectory").c_str(), tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", - (prefix + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (prefix + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance; } @@ -145,50 +137,49 @@ SegmentTolerances get_segment_tolerances( * \return True if \p state_error fulfills \p state_tolerance. */ inline bool check_state_tolerance_per_joint( - const trajectory_msgs::msg::JointTrajectoryPoint & state_error, - int joint_idx, - const StateTolerances & state_tolerance, - bool show_errors = false) + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, + const StateTolerances & state_tolerance, bool show_errors = false) { using std::abs; const double error_position = state_error.positions[joint_idx]; - const double error_velocity = state_error.velocities.empty() ? - 0.0 : state_error.velocities[joint_idx]; - const double error_acceleration = state_error.accelerations.empty() ? - 0.0 : state_error.accelerations[joint_idx]; + const double error_velocity = + state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; + const double error_acceleration = + state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; const bool is_valid = !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) && !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) && !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration); - if (is_valid) { + if (is_valid) + { return true; } - if (show_errors) { + if (show_errors) + { const auto logger = rclcpp::get_logger("tolerances"); RCLCPP_ERROR(logger, "Path state tolerances failed:"); - if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { + if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) + { RCLCPP_ERROR( - logger, - "Position Error: %f, Position Tolerance: %f", - error_position, state_tolerance.position); + logger, "Position Error: %f, Position Tolerance: %f", error_position, + state_tolerance.position); } - if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) { + if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) + { RCLCPP_ERROR( - logger, - "Velocity Error: %f, Velocity Tolerance: %f", - error_velocity, state_tolerance.velocity); + logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, + state_tolerance.velocity); } - if (state_tolerance.acceleration > 0.0 && - abs(error_acceleration) > state_tolerance.acceleration) + if ( + state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) { RCLCPP_ERROR( - logger, - "Acceleration Error: %f, Acceleration Tolerance: %f", - error_acceleration, state_tolerance.acceleration); + logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration, + state_tolerance.acceleration); } } return false; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d42838494a..36d6f0ffdd 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -24,9 +24,7 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace joint_trajectory_controller { - -using TrajectoryPointIter = - std::vector::iterator; +using TrajectoryPointIter = std::vector::iterator; using TrajectoryPointConstIter = std::vector::const_iterator; @@ -37,8 +35,7 @@ class Trajectory Trajectory(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - explicit Trajectory( - std::shared_ptr joint_trajectory); + explicit Trajectory(std::shared_ptr joint_trajectory); JOINT_TRAJECTORY_CONTROLLER_PUBLIC explicit Trajectory( @@ -51,14 +48,12 @@ class Trajectory /// from the current one, we call this function to log the current state, then /// append/replace the current trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void - set_point_before_trajectory_msg( + void set_point_before_trajectory_msg( const rclcpp::Time & current_time, const trajectory_msgs::msg::JointTrajectoryPoint & current_point); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void - update(std::shared_ptr joint_trajectory); + void update(std::shared_ptr joint_trajectory); /// Find the segment (made up of 2 points) and its expected state from the /// containing trajectory. @@ -76,12 +71,9 @@ class Trajectory * return false */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool - sample( - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & expected_state, - TrajectoryPointConstIter & start_segment_itr, - TrajectoryPointConstIter & end_segment_itr); + bool sample( + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); /** * Do interpolation between 2 states given a time in between their respective timestamps @@ -106,34 +98,31 @@ class Trajectory void interpolate_between_points( const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & output); + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - TrajectoryPointConstIter - begin() const; + TrajectoryPointConstIter begin() const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - TrajectoryPointConstIter - end() const; + TrajectoryPointConstIter end() const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time - time_from_start() const; + rclcpp::Time time_from_start() const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool - has_trajectory_msg() const; + bool has_trajectory_msg() const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - std::shared_ptr - get_trajectory_msg() const {return trajectory_msg_;} + std::shared_ptr get_trajectory_msg() const + { + return trajectory_msg_; + } JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const {return trajectory_start_time_;} + rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool is_sampled_already() const {return sampled_already_;} + bool is_sampled_already() const { return sampled_already_; } private: std::shared_ptr trajectory_msg_; @@ -150,18 +139,25 @@ class Trajectory * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is * "{2, 1}". */ -template +template inline std::vector mapping(const T & t1, const T & t2) { // t1 must be a subset of t2 - if (t1.size() > t2.size()) {return std::vector();} + if (t1.size() > t2.size()) + { + return std::vector(); + } std::vector mapping_vector(t1.size()); // Return value - for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) { + for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); - if (t2.end() == t2_it) { + if (t2.end() == t2_it) + { return std::vector(); - } else { + } + else + { const size_t t1_dist = std::distance(t1.begin(), t1_it); const size_t t2_dist = std::distance(t2.begin(), t2_it); mapping_vector[t1_dist] = t2_dist; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h b/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h index e41392302a..ec44da7a2f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__ ((dllexport)) - #define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__ ((dllimport)) - #else - #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(dllexport) - #define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(dllimport) - #endif - #ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT - #else - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT - #endif - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC - #define JOINT_TRAJECTORY_CONTROLLER_LOCAL +#ifdef __GNUC__ +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((dllexport)) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__((dllimport)) #else - #define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) - #define JOINT_TRAJECTORY_CONTROLLER_IMPORT - #if __GNUC__ >= 4 - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) - #define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC - #define JOINT_TRAJECTORY_CONTROLLER_LOCAL - #endif - #define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(dllexport) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT +#else +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL +#else +#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC +#define JOINT_TRAJECTORY_CONTROLLER_LOCAL +#endif +#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE #endif #endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index a7750242c0..133f832cf8 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -27,8 +27,6 @@ trajectory_msgs ament_cmake_gtest - ament_lint_auto - ament_lint_common controller_manager ros2_control_test_assets diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e6f77b631..7e39cba1be 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -44,18 +44,18 @@ namespace joint_trajectory_controller { - JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), - joint_names_({}) -{} +: controller_interface::ControllerInterface(), joint_names_({}) +{ +} -controller_interface::return_type -JointTrajectoryController::init(const std::string & controller_name) +controller_interface::return_type JointTrajectoryController::init( + const std::string & controller_name) { // initialize lifecycle node const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } @@ -80,8 +80,10 @@ JointTrajectoryController::command_interface_configuration() const controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) { - for (const auto & interface_type : command_interface_types_) { + for (const auto & joint_name : joint_names_) + { + for (const auto & interface_type : command_interface_types_) + { conf.names.push_back(joint_name + "/" + interface_type); } } @@ -94,39 +96,45 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) { - for (const auto & interface_type : state_interface_types_) { + for (const auto & joint_name : joint_names_) + { + for (const auto & interface_type : state_interface_types_) + { conf.names.push_back(joint_name + "/" + interface_type); } } return conf; } -controller_interface::return_type -JointTrajectoryController::update() +controller_interface::return_type JointTrajectoryController::update() { - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { return controller_interface::return_type::OK; } - auto compute_error_for_joint = [&](JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) + auto compute_error_for_joint = [&]( + JointTrajectoryPoint & error, int index, + const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) { + // error defined as the difference between current and desired + error.positions[index] = + angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (has_velocity_state_interface_ && has_velocity_command_interface_) { - // error defined as the difference between current and desired - error.positions[index] = angles::shortest_angular_distance( - current.positions[index], desired.positions[index]); - if (has_velocity_state_interface_ && has_velocity_command_interface_) { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; - } - }; + error.velocities[index] = desired.velocities[index] - current.velocities[index]; + } + if (has_acceleration_state_interface_ && has_acceleration_command_interface_) + { + error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + } + }; // Check if a new external message has been received from nonRT threads auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) { + if (current_external_msg != *new_external_msg) + { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); traj_external_point_ptr_->update(*new_external_msg); @@ -138,10 +146,10 @@ JointTrajectoryController::update() // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? - auto assign_interface_from_point = [&, joint_num]( - auto & joint_inteface, const std::vector & trajectory_point_interface) - { - for (auto index = 0ul; index < joint_num; ++index) { + auto assign_interface_from_point = + [&, joint_num](auto & joint_inteface, const std::vector & trajectory_point_interface) { + for (auto index = 0ul; index < joint_num; ++index) + { joint_inteface[index].get().set_value(trajectory_point_interface[index]); } }; @@ -151,15 +159,19 @@ JointTrajectoryController::update() read_state_from_hardware(state_current); // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { + if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + { // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) { - if (open_loop_control_) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg( - node_->now(), last_commanded_state_); - } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg( - node_->now(), state_current); + if (!(*traj_point_active_ptr_)->is_sampled_already()) + { + if (open_loop_control_) + { + (*traj_point_active_ptr_) + ->set_point_before_trajectory_msg(node_->now(), last_commanded_state_); + } + else + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(node_->now(), state_current); } } resize_joint_trajectory_point(state_error, joint_num); @@ -167,44 +179,51 @@ JointTrajectoryController::update() // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; // TODO(anyone): this is kind-of open-loop concept? I am right? - const bool valid_point = (*traj_point_active_ptr_)->sample( - node_->now(), state_desired, - start_segment_itr, end_segment_itr); + const bool valid_point = + (*traj_point_active_ptr_) + ->sample(node_->now(), state_desired, start_segment_itr, end_segment_itr); - if (valid_point) { + if (valid_point) + { bool abort = false; bool outside_goal_tolerance = false; const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); // set values for next hardware write() - if (has_position_command_interface_) { + if (has_position_command_interface_) + { assign_interface_from_point(joint_command_interface_[0], state_desired.positions); } - if (has_velocity_command_interface_) { + if (has_velocity_command_interface_) + { assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); } - if (has_acceleration_command_interface_) { + if (has_acceleration_command_interface_) + { assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); } // TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter" (see ROS1) - #171 -// if (check_if_interface_type_exist( -// command_interface_types_, hardware_interface::HW_IF_EFFORT)) { -// assign_interface_from_point(joint_command_interface_[3], state_desired.effort); -// } + // if (check_if_interface_type_exist( + // command_interface_types_, hardware_interface::HW_IF_EFFORT)) { + // assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + // } - for (auto index = 0ul; index < joint_num; ++index) { + for (auto index = 0ul; index < joint_num; ++index) + { compute_error_for_joint(state_error, index, state_current, state_desired); - if (before_last_point && !check_state_tolerance_per_joint( - state_error, index, - default_tolerances_.state_tolerance[index], false)) + if ( + before_last_point && + !check_state_tolerance_per_joint( + state_error, index, default_tolerances_.state_tolerance[index], false)) { abort = true; } // past the final point, check that we end up inside goal tolerance - if (!before_last_point && !check_state_tolerance_per_joint( - state_error, index, - default_tolerances_.goal_state_tolerance[index], false)) + if ( + !before_last_point && + !check_state_tolerance_per_joint( + state_error, index, default_tolerances_.goal_state_tolerance[index], false)) { outside_goal_tolerance = true; } @@ -214,7 +233,8 @@ JointTrajectoryController::update() last_commanded_state_ = state_desired; const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) { + if (active_goal) + { // send feedback auto feedback = std::make_shared(); feedback->header.stamp = node_->now(); @@ -226,13 +246,17 @@ JointTrajectoryController::update() active_goal->setFeedback(feedback); // check abort - if (abort || outside_goal_tolerance) { + if (abort || outside_goal_tolerance) + { auto result = std::make_shared(); - if (abort) { + if (abort) + { RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - } else if (outside_goal_tolerance) { + } + else if (outside_goal_tolerance) + { RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation"); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); } @@ -242,8 +266,11 @@ JointTrajectoryController::update() rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); // check goal tolerance - } else if (!before_last_point) { - if (!outside_goal_tolerance) { + } + else if (!before_last_point) + { + if (!outside_goal_tolerance) + { auto res = std::make_shared(); res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); active_goal->setSucceeded(res); @@ -252,13 +279,16 @@ JointTrajectoryController::update() rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); - } else if (default_tolerances_.goal_time_tolerance != 0.0) { + } + else if (default_tolerances_.goal_time_tolerance != 0.0) + { // if we exceed goal_time_toleralance set it to aborted const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; const double difference = node_->now().seconds() - traj_end.seconds(); - if (difference > default_tolerances_.goal_time_tolerance) { + if (difference > default_tolerances_.goal_time_tolerance) + { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); active_goal->setAborted(result); @@ -266,8 +296,7 @@ JointTrajectoryController::update() // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - node_->get_logger(), - "Aborted due goal_time_tolerance exceeding by %f seconds", + node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference); } } @@ -283,10 +312,10 @@ JointTrajectoryController::update() void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = [&, joint_num]( - std::vector & trajectory_point_interface, const auto & joint_inteface) - { - for (auto index = 0ul; index < joint_num; ++index) { + auto assign_point_from_interface = + [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { + for (auto index = 0ul; index < joint_num; ++index) + { trajectory_point_interface[index] = joint_inteface[index].get().get_value(); } }; @@ -295,16 +324,22 @@ void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & // Position states always exist assign_point_from_interface(state.positions, joint_state_interface_[0]); // velocity and acceleration states are optional - if (has_velocity_state_interface_) { + if (has_velocity_state_interface_) + { assign_point_from_interface(state.velocities, joint_state_interface_[1]); // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) { + if (has_acceleration_state_interface_) + { assign_point_from_interface(state.accelerations, joint_state_interface_[2]); - } else { + } + else + { // Make empty so the property is ignored during interpolation state.accelerations.clear(); } - } else { + } + else + { // Make empty so the property is ignored during interpolation state.velocities.clear(); state.accelerations.clear(); @@ -316,50 +351,63 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto bool has_values = true; const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = [&, joint_num]( - std::vector & trajectory_point_interface, const auto & joint_inteface) - { - for (auto index = 0ul; index < joint_num; ++index) { + auto assign_point_from_interface = + [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { + for (auto index = 0ul; index < joint_num; ++index) + { trajectory_point_interface[index] = joint_inteface[index].get().get_value(); } }; - auto interface_has_values = [](const auto & joint_interface) - { - return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) {return std::isnan(interface.get().get_value());}) == - joint_interface.end(); - }; + auto interface_has_values = [](const auto & joint_interface) { + return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { + return std::isnan(interface.get().get_value()); + }) == joint_interface.end(); + }; // Assign values from the command interfaces as state. Therefore needs check for both. // Position state interface has to exist always - if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) { + if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) + { assign_point_from_interface(state.positions, joint_command_interface_[0]); - } else { + } + else + { state.positions.clear(); has_values = false; } // velocity and acceleration states are optional - if (has_velocity_state_interface_) { - if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) { + if (has_velocity_state_interface_) + { + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) + { assign_point_from_interface(state.velocities, joint_command_interface_[1]); - } else { + } + else + { state.velocities.clear(); has_values = false; } - } else { + } + else + { state.velocities.clear(); } // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) { - if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) { + if (has_acceleration_state_interface_) + { + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) + { assign_point_from_interface(state.accelerations, joint_command_interface_[2]); - } else { + } + else + { state.accelerations.clear(); has_values = false; } - } else { + } + else + { state.accelerations.clear(); } @@ -374,20 +422,24 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // update parameters joint_names_ = node_->get_parameter("joints").as_string_array(); - if (!reset()) { + if (!reset()) + { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - if (joint_names_.empty()) { + if (joint_names_.empty()) + { RCLCPP_WARN(logger, "'joints' parameter is empty."); } // Specialized, child controllers set interfaces before calling configure function. - if (command_interface_types_.empty()) { + if (command_interface_types_.empty()) + { command_interface_types_ = node_->get_parameter("command_interfaces").as_string_array(); } - if (command_interface_types_.empty()) { + if (command_interface_types_.empty()) + { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); return CallbackReturn::FAILURE; } @@ -395,10 +447,12 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) { - auto it = std::find( - allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) { + for (const auto & interface : command_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); return CallbackReturn::FAILURE; } @@ -410,8 +464,10 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // 2. position [velocity, [acceleration]] // effort can't be combined with other interfaces - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) { - if (command_interface_types_.size() == 1) { + if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) + { + if (command_interface_types_.size() == 1) + { // TODO(anyone): This flag is not used for now // There should be PID-approach used as in ROS1: // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 @@ -419,40 +475,53 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // TODO(anyone): remove the next two lines when implemented RCLCPP_ERROR(logger, "using 'effort' command interface alone is not yet implemented yet."); return CallbackReturn::FAILURE; - } else { + } + else + { RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); return CallbackReturn::FAILURE; } } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION)) { + if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION)) + { has_position_command_interface_ = true; } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY)) + { has_velocity_command_interface_ = true; } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION)) { + if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION)) + { has_acceleration_command_interface_ = true; } - if (has_velocity_command_interface_) { + if (has_velocity_command_interface_) + { // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) { + if (command_interface_types_.size() == 1) + { use_closed_loop_pid_adapter = true; // TODO(anyone): remove this when implemented RCLCPP_ERROR(logger, "using 'velocity' command interface alone is not yet implemented yet."); return CallbackReturn::FAILURE; // if velocity interface can be used without position if multiple defined - } else if (!has_position_command_interface_) { + } + else if (!has_position_command_interface_) + { RCLCPP_ERROR( - logger, "'velocity' command interface can be used either alone or 'position' " + logger, + "'velocity' command interface can be used either alone or 'position' " "interface has to be present."); return CallbackReturn::FAILURE; } // invalid: acceleration is defined and no velocity - } else if (has_acceleration_command_interface_) { + } + else if (has_acceleration_command_interface_) + { RCLCPP_ERROR( - logger, "'acceleration' command interface can only be used if 'velocity' and " + logger, + "'acceleration' command interface can only be used if 'velocity' and " "'position' interfaces are present"); return CallbackReturn::FAILURE; } @@ -462,12 +531,14 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // Specialized, child controllers should set its default value. state_interface_types_ = node_->get_parameter("state_interfaces").as_string_array(); - if (state_interface_types_.empty()) { + if (state_interface_types_.empty()) + { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); return CallbackReturn::FAILURE; } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) { + if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) + { RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); return CallbackReturn::FAILURE; } @@ -476,46 +547,58 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // allocation during activation // Note: 'effort' storage is also here, but never used. Still, for this is OK. joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) { - auto it = std::find( - allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) { + for (const auto & interface : state_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); return CallbackReturn::FAILURE; } } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) + { has_velocity_state_interface_ = true; } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION)) { + if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION)) + { has_acceleration_state_interface_ = true; } - if (has_velocity_state_interface_) { - if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION)) { + if (has_velocity_state_interface_) + { + if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION)) + { RCLCPP_ERROR( - logger, "'velocity' state interface cannot be used if 'position' interface " + logger, + "'velocity' state interface cannot be used if 'position' interface " "is missing."); return CallbackReturn::FAILURE; } - } else if (has_acceleration_state_interface_) { + } + else if (has_acceleration_state_interface_) + { RCLCPP_ERROR( - logger, "'acceleration' state interface cannot be used if 'position' and 'velocity' " + logger, + "'acceleration' state interface cannot be used if 'position' and 'velocity' " "interfaces are not present."); return CallbackReturn::FAILURE; } auto get_interface_list = [](const std::vector & interface_types) { - std::stringstream ss_command_interfaces; - for (size_t index = 0; index < interface_types.size(); ++index) { - if (index != 0) { - ss_command_interfaces << " "; - } - ss_command_interfaces << interface_types[index]; + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_command_interfaces << " "; } - return ss_command_interfaces.str(); - }; + ss_command_interfaces << interface_types[index]; + } + return ss_command_interfaces.str(); + }; // Print output so users can be sure the interface setup is correct RCLCPP_INFO( @@ -526,48 +609,45 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) default_tolerances_ = get_segment_tolerances(*node_, joint_names_); // Read parameters customizing controller for special cases - open_loop_control_ = - node_->get_parameter("open_loop_control").get_value(); + open_loop_control_ = node_->get_parameter("open_loop_control").get_value(); // subscriber callback // non realtime // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) - -> void + auto callback = [this](const std::shared_ptr msg) -> void { + if (!validate_trajectory_msg(*msg)) { - if (!validate_trajectory_msg(*msg)) { - return; - } + return; + } - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) { - add_new_trajectory_msg(msg); - } - }; + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) + { + add_new_trajectory_msg(msg); + } + }; // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = - node_->create_subscription( + joint_command_subscriber_ = node_->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = - node_->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO( - logger, "Controller state will be published at %.2f Hz.", state_publish_rate); - if (state_publish_rate > 0.0) { - state_publisher_period_ = - rclcpp::Duration::from_seconds(1.0 / state_publish_rate); - } else { + const double state_publish_rate = node_->get_parameter("state_publish_rate").get_value(); + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); + if (state_publish_rate > 0.0) + { + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate); + } + else + { state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = node_->create_publisher( - "~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); const auto n_joints = joint_names_.size(); @@ -579,11 +659,13 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) state_publisher_->msg_.desired.accelerations.resize(n_joints); state_publisher_->msg_.actual.positions.resize(n_joints); state_publisher_->msg_.error.positions.resize(n_joints); - if (has_velocity_state_interface_) { + if (has_velocity_state_interface_) + { state_publisher_->msg_.actual.velocities.resize(n_joints); state_publisher_->msg_.error.velocities.resize(n_joints); } - if (has_acceleration_state_interface_) { + if (has_acceleration_state_interface_) + { state_publisher_->msg_.actual.accelerations.resize(n_joints); state_publisher_->msg_.error.accelerations.resize(n_joints); } @@ -592,45 +674,43 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) last_state_publish_time_ = node_->now(); // action server configuration - allow_partial_joints_goal_ = node_->get_parameter("allow_partial_joints_goal") - .get_value(); - if (allow_partial_joints_goal_) { + allow_partial_joints_goal_ = node_->get_parameter("allow_partial_joints_goal").get_value(); + if (allow_partial_joints_goal_) + { RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } - const double action_monitor_rate = node_->get_parameter("action_monitor_rate") - .get_value(); + const double action_monitor_rate = + node_->get_parameter("action_monitor_rate").get_value(); - RCLCPP_INFO( - logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); + RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), node_->get_node_waitables_interface(), std::string(node_->get_name()) + "/follow_joint_trajectory", std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1) - ); + std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } // Fill ordered_interfaces with references to the matching interfaces // in the same order as in joint_names -template +template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & joint_names, const std::string & interface_type, std::vector> & ordered_interfaces) { - for (const auto & joint_name : joint_names) { - for (auto & interface : unordered_interfaces) { - if ((interface.get_name() == joint_name) && - (interface.get_interface_name() == interface_type)) + for (const auto & joint_name : joint_names) + { + for (auto & interface : unordered_interfaces) + { + if ( + (interface.get_name() == joint_name) && (interface.get_interface_name() == interface_type)) { ordered_interfaces.emplace_back(std::ref(interface)); } @@ -644,29 +724,31 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { // order all joints in the storage - for (const auto & interface : command_interface_types_) { - auto it = std::find( - allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + for (const auto & interface : command_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), + interface.c_str(), joint_command_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - for (const auto & interface : state_interface_types_) { - auto it = std::find( - allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + for (const auto & interface : state_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), + interface.c_str(), joint_state_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } @@ -679,7 +761,8 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) traj_msg_home_ptr_->points[0].time_from_start.sec = 0; traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) { + for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) + { traj_msg_home_ptr_->points[0].positions[index] = joint_state_interface_[0][index].get().get_value(); } @@ -700,7 +783,8 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) // those are not nan trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, joint_names_.size()); - if (read_state_from_command_interfaces(state)) { + if (read_state_from_command_interfaces(state)) + { last_commanded_state_ = state; } @@ -712,12 +796,14 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) { // TODO(anyone): How to halt when using effort commands? - for (auto index = 0ul; index < joint_names_.size(); ++index) { + for (auto index = 0ul; index < joint_names_.size(); ++index) + { joint_command_interface_[0][index].get().set_value( joint_command_interface_[0][index].get().get_value()); } - for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) { + for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) + { joint_command_interface_[index].clear(); joint_state_interface_[index].clear(); } @@ -741,14 +827,14 @@ JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointTrajectoryController::on_error(const rclcpp_lifecycle::State &) { - if (!reset()) { + if (!reset()) + { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool -JointTrajectoryController::reset() +bool JointTrajectoryController::reset() { subscriber_is_active_ = false; joint_command_subscriber_.reset(); @@ -772,18 +858,21 @@ JointTrajectoryController::on_shutdown(const rclcpp_lifecycle::State &) } void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error) { - if (state_publisher_period_.seconds() <= 0.0) { + if (state_publisher_period_.seconds() <= 0.0) + { return; } - if (node_->now() < (last_state_publish_time_ + state_publisher_period_)) { + if (node_->now() < (last_state_publish_time_ + state_publisher_period_)) + { return; } - if (state_publisher_ && state_publisher_->trylock()) { + if (state_publisher_ && state_publisher_->trylock()) + { last_state_publish_time_ = node_->now(); state_publisher_->msg_.header.stamp = last_state_publish_time_; state_publisher_->msg_.desired.positions = desired_state.positions; @@ -791,11 +880,13 @@ void JointTrajectoryController::publish_state( state_publisher_->msg_.desired.accelerations = desired_state.accelerations; state_publisher_->msg_.actual.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; - if (has_velocity_state_interface_) { + if (has_velocity_state_interface_) + { state_publisher_->msg_.actual.velocities = current_state.velocities; state_publisher_->msg_.error.velocities = state_error.velocities; } - if (has_acceleration_state_interface_) { + if (has_acceleration_state_interface_) + { state_publisher_->msg_.actual.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } @@ -805,20 +896,19 @@ void JointTrajectoryController::publish_state( } rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal) + const rclcpp_action::GoalUUID &, std::shared_ptr goal) { RCLCPP_INFO(node_->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_ERROR( - node_->get_logger(), - "Can't accept new action goals. Controller is not running."); + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } - if (!validate_trajectory_msg(goal->trajectory)) { + if (!validate_trajectory_msg(goal->trajectory)) + { return rclcpp_action::GoalResponse::REJECT; } @@ -833,14 +923,14 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( // Check that cancel request refers to currently active goal (if any) const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal && active_goal->gh_ == goal_handle) { + if (active_goal && active_goal->gh_ == goal_handle) + { // Controller uptime // Enter hold current position mode set_hold_position(); RCLCPP_DEBUG( - node_->get_logger(), - "Canceling active action goal because cancel callback received."); + node_->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -856,8 +946,8 @@ void JointTrajectoryController::feedback_setup_callback( // Update new trajectory { preempt_active_goal(); - auto traj_msg = std::make_shared( - goal_handle->get_goal()->trajectory); + auto traj_msg = + std::make_shared(goal_handle->get_goal()->trajectory); add_new_trajectory_msg(traj_msg); } @@ -879,15 +969,18 @@ void JointTrajectoryController::fill_partial_goal( { // joint names in the goal are a subset of existing joints, as checked in goal_callback // so if the size matches, the goal contains all controller joints - if (joint_names_.size() == trajectory_msg->joint_names.size()) { + if (joint_names_.size() == trajectory_msg->joint_names.size()) + { return; } trajectory_msg->joint_names.reserve(joint_names_.size()); - for (auto index = 0ul; index < joint_names_.size(); ++index) { + for (auto index = 0ul; index < joint_names_.size(); ++index) + { { - if (std::find( + if ( + std::find( trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), joint_names_[index]) != trajectory_msg->joint_names.end()) { @@ -896,16 +989,20 @@ void JointTrajectoryController::fill_partial_goal( } trajectory_msg->joint_names.push_back(joint_names_[index]); - for (auto & it : trajectory_msg->points) { + for (auto & it : trajectory_msg->points) + { // Assume hold position with 0 velocity and acceleration for missing joints it.positions.push_back(joint_command_interface_[0][index].get().get_value()); - if (!it.velocities.empty()) { + if (!it.velocities.empty()) + { it.velocities.push_back(0.0); } - if (!it.accelerations.empty()) { + if (!it.accelerations.empty()) + { it.accelerations.push_back(0.0); } - if (!it.effort.empty()) { + if (!it.effort.empty()) + { it.effort.push_back(0.0); } } @@ -918,29 +1015,30 @@ void JointTrajectoryController::sort_to_local_joint_order( { // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); - auto remap = [this](const std::vector & to_remap, const std::vector & mapping) - -> std::vector + auto remap = [this]( + const std::vector & to_remap, + const std::vector & mapping) -> std::vector { + if (to_remap.empty()) { - if (to_remap.empty()) { - return to_remap; - } - if (to_remap.size() != mapping.size()) { - RCLCPP_WARN( - node_->get_logger(), - "Invalid input size (%zu) for sorting", - to_remap.size()); - return to_remap; - } - std::vector output; - output.resize(mapping.size(), 0.0); - for (auto index = 0ul; index < mapping.size(); ++index) { - auto map_index = mapping[index]; - output[map_index] = to_remap[index]; - } - return output; - }; + return to_remap; + } + if (to_remap.size() != mapping.size()) + { + RCLCPP_WARN(node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + std::vector output; + output.resize(mapping.size(), 0.0); + for (auto index = 0ul; index < mapping.size(); ++index) + { + auto map_index = mapping[index]; + output[map_index] = to_remap[index]; + } + return output; + }; - for (auto index = 0ul; index < trajectory_msg->points.size(); ++index) { + for (auto index = 0ul; index < trajectory_msg->points.size(); ++index) + { trajectory_msg->points[index].positions = remap(trajectory_msg->points[index].positions, mapping_vector); @@ -956,41 +1054,40 @@ void JointTrajectoryController::sort_to_local_joint_order( } bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, - const std::vector & vector_field, + size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const { - if (allow_empty && vector_field.empty()) { + if (allow_empty && vector_field.empty()) + { return true; } - if (joint_names_size != vector_field.size()) { + if (joint_names_size != vector_field.size()) + { RCLCPP_ERROR( - node_->get_logger(), - "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; } - bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) { - if (trajectory.joint_names.size() != joint_names_.size()) { + if (!allow_partial_joints_goal_) + { + if (trajectory.joint_names.size() != joint_names_.size()) + { RCLCPP_ERROR( - node_->get_logger(), - "Joints on incoming trajectory don't match the controller joints."); + node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); return false; } } - if (trajectory.joint_names.empty()) { - RCLCPP_ERROR( - node_->get_logger(), - "Empty joint names on incoming trajectory."); + if (trajectory.joint_names.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Empty joint names on incoming trajectory."); return false; } @@ -998,12 +1095,15 @@ bool JointTrajectoryController::validate_trajectory_msg( // If the starting time it set to 0.0, it means the controller should start it now. // Otherwise we check if the trajectory ends before the current time, // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) { + if (trajectory_start_time.seconds() != 0.0) + { auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) { + for (const auto & p : trajectory.points) + { trajectory_end_time += p.time_from_start; } - if (trajectory_end_time < node_->now()) { + if (trajectory_end_time < node_->now()) + { RCLCPP_ERROR( node_->get_logger(), "Received trajectory with non zero time start time (%f) that ends on the past (%f)", @@ -1012,22 +1112,25 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) { + for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) + { const std::string & incoming_joint_name = trajectory.joint_names[i]; auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) { + if (it == joint_names_.end()) + { RCLCPP_ERROR( - node_->get_logger(), - "Incoming joint %s doesn't match the controller's joints.", + node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", incoming_joint_name.c_str()); return false; } } rclcpp::Duration previous_traj_time(0ms); - for (auto i = 0ul; i < trajectory.points.size(); ++i) { - if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { + for (auto i = 0ul; i < trajectory.points.size(); ++i) + { + if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) + { RCLCPP_ERROR( node_->get_logger(), "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", @@ -1039,11 +1142,11 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; - if (!validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || + if ( + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, - true) || + joint_count, points[i].accelerations, "accelerations", i, true) || !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) { return false; @@ -1061,7 +1164,8 @@ void JointTrajectoryController::add_new_trajectory_msg( void JointTrajectoryController::preempt_active_goal() { const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal) { + if (active_goal) + { auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); @@ -1075,8 +1179,7 @@ void JointTrajectoryController::set_hold_position() trajectory_msgs::msg::JointTrajectory empty_msg; empty_msg.header.stamp = rclcpp::Time(0); - auto traj_msg = std::make_shared( - empty_msg); + auto traj_msg = std::make_shared(empty_msg); add_new_trajectory_msg(traj_msg); } @@ -1091,10 +1194,12 @@ void JointTrajectoryController::resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) { point.positions.resize(size); - if (has_velocity_state_interface_) { + if (has_velocity_state_interface_) + { point.velocities.resize(size); } - if (has_acceleration_state_interface_) { + if (has_acceleration_state_interface_) + { point.accelerations.resize(size); } } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 10b7229e41..83ba25d2d6 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -23,13 +23,9 @@ #include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { +Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} -Trajectory::Trajectory() -: trajectory_start_time_(0), time_before_traj_msg_(0) -{} - -Trajectory::Trajectory( - std::shared_ptr joint_trajectory) +Trajectory::Trajectory(std::shared_ptr joint_trajectory) : trajectory_msg_(joint_trajectory), trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) { @@ -46,8 +42,7 @@ Trajectory::Trajectory( update(joint_trajectory); } -void -Trajectory::set_point_before_trajectory_msg( +void Trajectory::set_point_before_trajectory_msg( const rclcpp::Time & current_time, const trajectory_msgs::msg::JointTrajectoryPoint & current_point) { @@ -55,33 +50,32 @@ Trajectory::set_point_before_trajectory_msg( state_before_traj_msg_ = current_point; } -void -Trajectory::update(std::shared_ptr joint_trajectory) +void Trajectory::update(std::shared_ptr joint_trajectory) { trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; } -bool -Trajectory::sample( - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & expected_state, - TrajectoryPointConstIter & start_segment_itr, - TrajectoryPointConstIter & end_segment_itr) +bool Trajectory::sample( + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) expected_state = trajectory_msgs::msg::JointTrajectoryPoint(); - if (trajectory_msg_->points.empty()) { + if (trajectory_msg_->points.empty()) + { start_segment_itr = end(); end_segment_itr = end(); return false; } // first sampling of this trajectory - if (!sampled_already_) { - if (trajectory_start_time_.seconds() == 0.0) { + if (!sampled_already_) + { + if (trajectory_start_time_.seconds() == 0.0) + { trajectory_start_time_ = sample_time; } @@ -89,7 +83,8 @@ Trajectory::sample( } // sampling before the current point - if (sample_time < time_before_traj_msg_) { + if (sample_time < time_before_traj_msg_) + { return false; } @@ -97,12 +92,13 @@ Trajectory::sample( const auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Duration offset = first_point_in_msg.time_from_start; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + offset; - if (sample_time < first_point_timestamp) { + if (sample_time < first_point_timestamp) + { const rclcpp::Time t0 = time_before_traj_msg_; interpolate_between_points( - t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, - sample_time, expected_state); + t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, + expected_state); start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); return true; @@ -110,7 +106,8 @@ Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (auto i = 0ul; i < last_idx; ++i) { + for (auto i = 0ul; i < last_idx; ++i) + { const auto & point = trajectory_msg_->points[i]; const auto & next_point = trajectory_msg_->points[i + 1]; @@ -119,7 +116,8 @@ Trajectory::sample( const rclcpp::Time t0 = trajectory_start_time_ + t0_offset; const rclcpp::Time t1 = trajectory_start_time_ + t1_offset; - if (sample_time >= t0 && sample_time < t1) { + if (sample_time >= t0 && sample_time < t1) + { interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state); start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); @@ -132,10 +130,12 @@ Trajectory::sample( end_segment_itr = end(); expected_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them - if (expected_state.velocities.empty()) { + if (expected_state.velocities.empty()) + { expected_state.velocities.resize(expected_state.positions.size(), 0.0); } - if (expected_state.accelerations.empty()) { + if (expected_state.accelerations.empty()) + { expected_state.accelerations.resize(expected_state.positions.size(), 0.0); } return true; @@ -144,8 +144,7 @@ Trajectory::sample( void Trajectory::interpolate_between_points( const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, - const rclcpp::Time & sample_time, - trajectory_msgs::msg::JointTrajectoryPoint & output) + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output) { rclcpp::Duration duration_so_far = sample_time - time_a; rclcpp::Duration duration_btwn_points = time_b - time_a; @@ -155,21 +154,23 @@ void Trajectory::interpolate_between_points( output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); - auto generate_powers = [](int n, double x, double * powers) + auto generate_powers = [](int n, double x, double * powers) { + powers[0] = 1.0; + for (int i = 1; i <= n; ++i) { - powers[0] = 1.0; - for (int i = 1; i <= n; ++i) { - powers[i] = powers[i - 1] * x; - } - }; + powers[i] = powers[i - 1] * x; + } + }; bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); - if (duration_so_far.seconds() < 0.0) { + if (duration_so_far.seconds() < 0.0) + { duration_so_far = rclcpp::Duration::from_seconds(0.0); has_velocity = has_accel = false; } - if (duration_so_far.seconds() > duration_btwn_points.seconds()) { + if (duration_so_far.seconds() > duration_btwn_points.seconds()) + { duration_so_far = duration_btwn_points; has_velocity = has_accel = false; } @@ -177,28 +178,33 @@ void Trajectory::interpolate_between_points( double t[6]; generate_powers(5, duration_so_far.seconds(), t); - if (!has_velocity && !has_accel) { + if (!has_velocity && !has_accel) + { // do linear interpolation - for (size_t i = 0; i < dim; ++i) { + for (size_t i = 0; i < dim; ++i) + { double start_pos = state_a.positions[i]; double end_pos = state_b.positions[i]; double coefficients[2] = {0.0, 0.0}; coefficients[0] = start_pos; - if (duration_btwn_points.seconds() != 0.0) { + if (duration_btwn_points.seconds() != 0.0) + { coefficients[1] = (end_pos - start_pos) / duration_btwn_points.seconds(); } - output.positions[i] = t[0] * coefficients[0] + - t[1] * coefficients[1]; + output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1]; output.velocities[i] = t[0] * coefficients[1]; } - } else if (has_velocity && !has_accel) { + } + else if (has_velocity && !has_accel) + { // do cubic interpolation double T[4]; generate_powers(3, duration_btwn_points.seconds(), T); - for (size_t i = 0; i < dim; ++i) { + for (size_t i = 0; i < dim; ++i) + { double start_pos = state_a.positions[i]; double start_vel = state_a.velocities[i]; double end_pos = state_b.positions[i]; @@ -207,29 +213,29 @@ void Trajectory::interpolate_between_points( double coefficients[4] = {0.0, 0.0, 0.0, 0.0}; coefficients[0] = start_pos; coefficients[1] = start_vel; - if (duration_btwn_points.seconds() != 0.0) { + if (duration_btwn_points.seconds() != 0.0) + { coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2]; coefficients[3] = - ( 2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3]; + (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3]; } - output.positions[i] = t[0] * coefficients[0] + - t[1] * coefficients[1] + - t[2] * coefficients[2] + - t[3] * coefficients[3]; - output.velocities[i] = t[0] * coefficients[1] + - t[1] * 2.0 * coefficients[2] + - t[2] * 3.0 * coefficients[3]; - output.accelerations[i] = t[0] * 2.0 * coefficients[2] + - t[1] * 6.0 * coefficients[3]; + output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] + + t[2] * coefficients[2] + t[3] * coefficients[3]; + output.velocities[i] = + t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3]; + output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3]; } - } else if (has_velocity && has_accel) { + } + else if (has_velocity && has_accel) + { // do quintic interpolation double T[6]; generate_powers(5, duration_btwn_points.seconds(), T); - for (size_t i = 0; i < dim; ++i) { + for (size_t i = 0; i < dim; ++i) + { double start_pos = state_a.positions[i]; double start_vel = state_a.velocities[i]; double start_acc = state_a.accelerations[i]; @@ -241,62 +247,47 @@ void Trajectory::interpolate_between_points( coefficients[0] = start_pos; coefficients[1] = start_vel; coefficients[2] = 0.5 * start_acc; - if (duration_btwn_points.seconds() != 0.0) { - coefficients[3] = - (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] - - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]); - coefficients[4] = - (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2] + - 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]); + if (duration_btwn_points.seconds() != 0.0) + { + coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + + end_acc * T[2] - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / + (2.0 * T[3]); + coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - + 2.0 * end_acc * T[2] + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / + (2.0 * T[4]); coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - - 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]); + 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / + (2.0 * T[5]); } - output.positions[i] = t[0] * coefficients[0] + - t[1] * coefficients[1] + - t[2] * coefficients[2] + - t[3] * coefficients[3] + - t[4] * coefficients[4] + - t[5] * coefficients[5]; - output.velocities[i] = t[0] * coefficients[1] + - t[1] * 2.0 * coefficients[2] + - t[2] * 3.0 * coefficients[3] + - t[3] * 4.0 * coefficients[4] + - t[4] * 5.0 * coefficients[5]; - output.accelerations[i] = t[0] * 2.0 * coefficients[2] + - t[1] * 6.0 * coefficients[3] + - t[2] * 12.0 * coefficients[4] + - t[3] * 20.0 * coefficients[5]; + output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] + + t[2] * coefficients[2] + t[3] * coefficients[3] + + t[4] * coefficients[4] + t[5] * coefficients[5]; + output.velocities[i] = t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + + t[2] * 3.0 * coefficients[3] + t[3] * 4.0 * coefficients[4] + + t[4] * 5.0 * coefficients[5]; + output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] + + t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5]; } } } -TrajectoryPointConstIter -Trajectory::begin() const +TrajectoryPointConstIter Trajectory::begin() const { THROW_ON_NULLPTR(trajectory_msg_) return trajectory_msg_->points.begin(); } -TrajectoryPointConstIter -Trajectory::end() const +TrajectoryPointConstIter Trajectory::end() const { THROW_ON_NULLPTR(trajectory_msg_) return trajectory_msg_->points.end(); } -rclcpp::Time -Trajectory::time_from_start() const -{ - return trajectory_start_time_; -} +rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_; } -bool -Trajectory::has_trajectory_msg() const -{ - return trajectory_msg_.get() != nullptr; -} +bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index a793f53a4b..6a08e62e99 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -20,8 +20,8 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" -#include "test_trajectory_controller_utils.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "test_trajectory_controller_utils.hpp" TEST(TestLoadJointStateController, load_controller) { @@ -32,12 +32,11 @@ TEST(TestLoadJointStateController, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_trajectory_controller", - "joint_trajectory_controller/JointTrajectoryController")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController")); rclcpp::shutdown(); } diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6b47e473f0..ff820be446 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -34,8 +34,8 @@ using namespace std::chrono_literals; // Floating-point value comparison threshold const double EPS = 1e-8; - -TEST(TestTrajectory, initialize_trajectory) { +TEST(TestTrajectory, initialize_trajectory) +{ { auto empty_msg = std::make_shared(); empty_msg->header.stamp.sec = 2; @@ -70,7 +70,8 @@ TEST(TestTrajectory, initialize_trajectory) { } } -TEST(TestTrajectory, sample_trajectory_positions) { +TEST(TestTrajectory, sample_trajectory_positions) +{ auto full_msg = std::make_shared(); full_msg->header.stamp = rclcpp::Time(0); @@ -172,7 +173,8 @@ TEST(TestTrajectory, sample_trajectory_positions) { } } -TEST(TestTrajectory, interpolation_pos_vel) { +TEST(TestTrajectory, interpolation_pos_vel) +{ // taken from ros1_controllers QuinticSplineSegmentTest::PosVelEnpointsSampler // Start and end state taken from x^3 - 2x @@ -196,9 +198,8 @@ TEST(TestTrajectory, interpolation_pos_vel) { // sample at start_time { traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + start_state.time_from_start, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start, expected_state); EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); @@ -208,9 +209,8 @@ TEST(TestTrajectory, interpolation_pos_vel) { { auto t = rclcpp::Duration::from_seconds(std::sqrt(2.0)); traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + start_state.time_from_start + t, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start + t, expected_state); EXPECT_NEAR(0.0, expected_state.positions[0], EPS); EXPECT_NEAR(4.0, expected_state.velocities[0], EPS); EXPECT_NEAR(6.0 * std::sqrt(2.0), expected_state.accelerations[0], EPS); @@ -219,16 +219,16 @@ TEST(TestTrajectory, interpolation_pos_vel) { // sample at end_time { traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + end_state.time_from_start, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + end_state.time_from_start, expected_state); EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(12.0, expected_state.accelerations[0], EPS); } } -TEST(TestTrajectory, interpolation_pos_vel_accel) { +TEST(TestTrajectory, interpolation_pos_vel_accel) +{ // taken from ros1_controllers QuinticSplineSegmentTest::PosVeAcclEnpointsSampler // Start and end state taken from x(x-1)(x-2)(x-3)(x-4) = x^5 -10x^4 + 35x^3 -50x^2 + 24x @@ -252,9 +252,8 @@ TEST(TestTrajectory, interpolation_pos_vel_accel) { // sample at start_time { traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + start_state.time_from_start, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start, expected_state); EXPECT_NEAR(start_state.positions[0], expected_state.positions[0], EPS); EXPECT_NEAR(start_state.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(start_state.accelerations[0], expected_state.accelerations[0], EPS); @@ -264,9 +263,8 @@ TEST(TestTrajectory, interpolation_pos_vel_accel) { { auto t = rclcpp::Duration::from_seconds(1.0); traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + start_state.time_from_start + t, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + start_state.time_from_start + t, expected_state); EXPECT_NEAR(0.0, expected_state.positions[0], EPS); EXPECT_NEAR(-6.0, expected_state.velocities[0], EPS); EXPECT_NEAR(10.0, expected_state.accelerations[0], EPS); @@ -275,9 +273,8 @@ TEST(TestTrajectory, interpolation_pos_vel_accel) { // sample at end_time { traj.interpolate_between_points( - time_now + start_state.time_from_start, start_state, - time_now + end_state.time_from_start, end_state, - time_now + end_state.time_from_start, expected_state); + time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start, + end_state, time_now + end_state.time_from_start, expected_state); EXPECT_NEAR(end_state.positions[0], expected_state.positions[0], EPS); EXPECT_NEAR(end_state.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(end_state.accelerations[0], expected_state.accelerations[0], EPS); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index d301d3bd2d..89195279e0 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -27,10 +27,10 @@ #include #include -#include "gtest/gtest.h" #include "action_msgs/msg/goal_status_array.hpp" #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" #include "controller_interface/controller_interface.hpp" +#include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" @@ -49,11 +49,11 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -using trajectory_msgs::msg::JointTrajectoryPoint; -using test_trajectory_controllers::TestableJointTrajectoryController; -using test_trajectory_controllers::TrajectoryControllerTest; using std::placeholders::_1; using std::placeholders::_2; +using test_trajectory_controllers::TestableJointTrajectoryController; +using test_trajectory_controllers::TrajectoryControllerTest; +using trajectory_msgs::msg::JointTrajectoryPoint; class TestTrajectoryActions : public TrajectoryControllerTest { @@ -82,26 +82,23 @@ class TestTrajectoryActions : public TrajectoryControllerTest executor_->add_node(node_->get_node_base_interface()); - executor_future_handle_ = std::async( - std::launch::async, [&]() -> void { - executor_->spin(); - }); + executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); }); } void SetUpControllerHardware() { setup_controller_hw_ = true; - controller_hw_thread_ = std::thread( - [&]() { - // controller hardware cycle update loop - auto start_time = rclcpp::Clock().now(); - rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); - } - }); + controller_hw_thread_ = std::thread([&]() { + // controller hardware cycle update loop + auto start_time = rclcpp::Clock().now(); + rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); + auto end_time = start_time + wait; + while (rclcpp::Clock().now() < end_time) + { + traj_controller_->update(); + } + }); // common_goal_response and common_result_response // sometimes doesn't receive calls when we don't sleep @@ -111,23 +108,18 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpActionClient() { action_client_ = rclcpp_action::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), + node_->get_node_base_interface(), node_->get_node_graph_interface(), + node_->get_node_logging_interface(), node_->get_node_waitables_interface(), controller_name_ + "/follow_joint_trajectory"); - bool response = - action_client_->wait_for_action_server(std::chrono::seconds(1)); - if (!response) { + bool response = action_client_->wait_for_action_server(std::chrono::seconds(1)); + if (!response) + { throw std::runtime_error("could not get action server"); } } - static void TearDownTestCase() - { - rclcpp::shutdown(); - } + static void TearDownTestCase() { rclcpp::shutdown(); } void TearDown() { @@ -137,7 +129,8 @@ class TestTrajectoryActions : public TrajectoryControllerTest void TearDownExecutor() { - if (setup_executor_) { + if (setup_executor_) + { setup_executor_ = false; executor_->cancel(); executor_future_handle_.wait(); @@ -146,9 +139,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest void TearDownControllerHardware() { - if (setup_controller_hw_) { + if (setup_controller_hw_) + { setup_controller_hw_ = false; - if (controller_hw_thread_.joinable()) { + if (controller_hw_thread_.joinable()) + { controller_hw_thread_.join(); } } @@ -159,9 +154,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; bool sendActionGoal( - const std::vector & points, - double timeout, - const GoalOptions & opt) + const std::vector & points, double timeout, const GoalOptions & opt) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); @@ -190,13 +183,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest void common_goal_response(std::shared_future future) { RCLCPP_DEBUG( - node_->get_logger(), "common_goal_response time: %f", - rclcpp::Clock().now().seconds()); + node_->get_logger(), "common_goal_response time: %f", rclcpp::Clock().now().seconds()); const auto goal_handle = future.get(); - if (!goal_handle) { + if (!goal_handle) + { common_goal_accepted_ = false; RCLCPP_DEBUG(node_->get_logger(), "Goal rejected"); - } else { + } + else + { common_goal_accepted_ = true; RCLCPP_DEBUG(node_->get_logger(), "Goal accepted"); } @@ -205,11 +200,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest void common_result_response(const GoalHandle::WrappedResult & result) { RCLCPP_DEBUG( - node_->get_logger(), "common_result_response time: %f", - rclcpp::Clock().now().seconds()); + node_->get_logger(), "common_result_response time: %f", rclcpp::Clock().now().seconds()); common_resultcode_ = result.code; common_action_result_code_ = result.result->error_code; - switch (result.code) { + switch (result.code) + { case rclcpp_action::ResultCode::SUCCEEDED: break; case rclcpp_action::ResultCode::ABORTED: @@ -225,7 +220,8 @@ class TestTrajectoryActions : public TrajectoryControllerTest } }; -TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { +TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) +{ SetUpExecutor(); SetUpControllerHardware(); @@ -254,18 +250,17 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { EXPECT_EQ(3.0, joint_pos_[2]); } -TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { +TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) +{ SetUpExecutor(); SetUpControllerHardware(); // add feedback bool feedback_recv = false; - goal_options_.feedback_callback = [&]( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr) - { - feedback_recv = true; - }; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; // send goal with multiple points { @@ -301,13 +296,13 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); } -TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { +TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) +{ // set tolerance parameters - std::vector params = - {rclcpp::Parameter("constraints.joint1.goal", 0.1), rclcpp::Parameter( - "constraints.joint2.goal", - 0.1), rclcpp::Parameter( - "constraints.joint3.goal", 0.1)}; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -331,33 +326,30 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { EXPECT_TRUE(common_goal_accepted_); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, - common_action_result_code_); + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); } -TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { +TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) +{ // set tolerance parameters - std::vector params = - {rclcpp::Parameter("constraints.joint1.goal", 0.1), rclcpp::Parameter( - "constraints.joint2.goal", - 0.1), rclcpp::Parameter( - "constraints.joint3.goal", 0.1)}; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1)}; SetUpExecutor(params); SetUpControllerHardware(); // add feedback bool feedback_recv = false; - goal_options_.feedback_callback = [&]( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr) - { - feedback_recv = true; - }; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; // send goal with multiple points { @@ -388,23 +380,21 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { EXPECT_TRUE(common_goal_accepted_); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, - common_action_result_code_); + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); } -TEST_F(TestTrajectoryActions, test_state_tolerances_fail) { +TEST_F(TestTrajectoryActions, test_state_tolerances_fail) +{ // set joint tolerance parameters const double state_tol = 0.0001; - std::vector params = {rclcpp::Parameter( - "constraints.joint1.trajectory", - state_tol), rclcpp::Parameter( - "constraints.joint2.trajectory", state_tol), rclcpp::Parameter( - "constraints.joint3.trajectory", state_tol)}; - + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -432,7 +422,8 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) { common_action_result_code_); } -TEST_F(TestTrajectoryActions, test_cancel_hold_position) { +TEST_F(TestTrajectoryActions, test_cancel_hold_position) +{ SetUpExecutor(); SetUpControllerHardware(); @@ -465,8 +456,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) { EXPECT_TRUE(common_goal_accepted_); EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, - common_action_result_code_); + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); const double prev_pos1 = joint_pos_[0]; const double prev_pos2 = joint_pos_[1]; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 44a88dfbe5..5f29d38ebb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include @@ -59,11 +58,7 @@ using test_trajectory_controllers::TestableJointTrajectoryController; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -void spin(rclcpp::executors::MultiThreadedExecutor * exe) -{ - exe->spin(); -} - +void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure) { @@ -80,7 +75,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - std::vector> points {{{3.3, 4.4, 5.5}}}; + std::vector> points{{{3.3, 4.4, 5.5}}}; publish(time_from_start, points); std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -106,13 +101,11 @@ TEST_P(TrajectoryControllerTestParameterized, activate) auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( - cmd_interface_config.names.size(), - joint_names_.size() * command_interface_types_.size()); + cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); auto state_interface_config = traj_controller_->state_interface_configuration(); ASSERT_EQ( - state_interface_config.names.size(), - joint_names_.size() * state_interface_types_.size()); + state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); ActivateTrajectoryController(); ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); @@ -245,7 +238,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - std::vector> points {{{3.3, 4.4, 5.5}}}; + std::vector> points{{{3.3, 4.4, 5.5}}}; publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); traj_controller_->update(); @@ -292,11 +285,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* - std::vector> points { - {{3.3, 4.4, 5.5}}, - {{7.7, 8.8, 9.9}}, - {{10.10, 11.11, 12.12}} - }; + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; // *INDENT-ON* publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); @@ -353,7 +343,8 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) size_t n_joints = joint_names_.size(); - for (unsigned int i = 0; i < n_joints; ++i) { + for (unsigned int i = 0; i < n_joints; ++i) + { EXPECT_EQ(joint_names_[i], state->joint_names[i]); } @@ -363,18 +354,24 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->desired.accelerations.empty()); EXPECT_EQ(n_joints, state->actual.positions.size()); - if (std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { EXPECT_TRUE(state->actual.velocities.empty()); - } else { + } + else + { EXPECT_EQ(n_joints, state->actual.velocities.size()); } - if (std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { EXPECT_TRUE(state->actual.accelerations.empty()); - } else { + } + else + { EXPECT_EQ(n_joints, state->actual.accelerations.size()); } @@ -386,15 +383,11 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", - static_cast(target_msg_count)); + "state_publish_rate", static_cast(target_msg_count)); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); - auto future_handle = std::async( - std::launch::async, [&executor]() -> void { - executor.spin(); - }); + auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); using control_msgs::msg::JointTrajectoryControllerState; @@ -402,18 +395,15 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou int echo_received_counter = 0; rclcpp::Subscription::SharedPtr subs = traj_node_->create_subscription( - controller_name_ + "/state", - qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { - ++echo_received_counter; - } - ); + controller_name_ + "/state", qos_level, + [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); // update for 1second const auto start_time = rclcpp::Clock().now(); const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); const auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) { + while (rclcpp::Clock().now() < end_time) + { traj_controller_->update(); } @@ -445,9 +435,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(true, {}, &executor); { trajectory_msgs::msg::JointTrajectory traj_msg; - const std::vector jumbled_joint_names { - joint_names_[1], joint_names_[2], joint_names_[0] - }; + const std::vector jumbled_joint_names{ + joint_names_[1], joint_names_[2], joint_names_[0]}; traj_msg.joint_names = jumbled_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); @@ -486,9 +475,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names { - joint_names_[1], joint_names_[0] - }; + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); @@ -511,18 +498,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) double threshold = 0.001; EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR( - initial_joint3_cmd, joint_pos_[2], - threshold) << "Joint 3 command should be current position"; + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "Joint 3 command should be current position"; - if (std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != command_interface_types_.end()) { // TODO(anyone): need help here - we should at least estimate the sign of the velocity -// EXPECT_NEAR(traj_msg.points[0].velocities[1], joint_vel_[0], threshold); -// EXPECT_NEAR(traj_msg.points[0].velocities[0], joint_vel_[1], threshold); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) << - "Joint 3 velocity should be 0.0 since it's not in the goal"; + // EXPECT_NEAR(traj_msg.points[0].velocities[1], joint_vel_[0], threshold); + // EXPECT_NEAR(traj_msg.points[0].velocities[0], joint_vel_[1], threshold); + EXPECT_NEAR(0.0, joint_vel_[2], threshold) + << "Joint 3 velocity should be 0.0 since it's not in the goal"; } // TODO(anyone): add here ckecks for acceleration commands @@ -547,9 +534,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names { - joint_names_[1], joint_names_[0] - }; + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); @@ -570,42 +555,40 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe updateController(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; - EXPECT_NEAR( - initial_joint1_cmd, joint_pos_[0], - threshold) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR( - initial_joint2_cmd, joint_pos_[1], - threshold) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR( - initial_joint3_cmd, joint_pos_[2], - threshold) << "All joints command should be current position because goal was rejected"; - - if (std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "All joints command should be current position because goal was rejected"; + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != command_interface_types_.end()) { - EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) << - "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) << - "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) << - "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; } - if (std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != command_interface_types_.end()) { - EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) << - "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) << - "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) << - "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; } executor.cancel(); } - /** * @brief invalid_message Test mismatched joint and reference vector sizes */ @@ -680,8 +663,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) subscribeToState(); - std::vector> points_old {{{2., 3., 4.}}}; - std::vector> points_partial_new {{1.5}}; + std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_partial_new{{1.5}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; @@ -713,8 +696,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations - std::vector> points_old {{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new {{{-1., -2., -3.}}}; + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; @@ -734,13 +717,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } -TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { +TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +{ rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {}, &executor); subscribeToState(); - std::vector> points_old {{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new {{{-1., -2., -3.}, {-2., -4., -6.}}}; + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; @@ -760,7 +744,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } - TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) { SetUpTrajectoryController(); @@ -780,8 +763,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur traj_controller_->configure(); traj_controller_->activate(); - std::vector> full_traj {{{2., 3., 4.}, {4., 6., 8.}}}; - std::vector> partial_traj {{{-1., -2.}, {-2., -4, }}}; + std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> partial_traj{ + {{-1., -2.}, + { + -2., + -4, + }}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; // Send full trajectory @@ -794,7 +782,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // Check that we reached end of points_old[0]trajectory and are starting points_old[1] waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); publish(points_delay, partial_traj, rclcpp::Clock().now() + delay * 2); @@ -804,8 +791,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration( - delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); } TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) @@ -824,7 +810,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - std::vector> points {{first_goal}}; + std::vector> points{{first_goal}}; publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); @@ -892,8 +878,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro executor.cancel(); } -TEST_P( - TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter @@ -909,7 +894,7 @@ TEST_P( builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - std::vector> points {{first_goal}}; + std::vector> points{{first_goal}}; publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); @@ -979,15 +964,15 @@ TEST_P( // 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 -TEST_P( - TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +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 (auto i = 0u; i < 3; ++i) { + for (auto i = 0u; 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(); @@ -996,11 +981,13 @@ TEST_P( auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - for (auto i = 0u; i < 3; ++i) { + for (auto i = 0u; i < 3; ++i) + { EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); // check velocity - if (std::find( + if ( + std::find( state_interface_types_.begin(), state_interface_types_.end(), hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && std::find( @@ -1011,7 +998,8 @@ TEST_P( } // check acceleration - if (std::find( + if ( + std::find( state_interface_types_.begin(), state_interface_types_.end(), hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && std::find( @@ -1027,15 +1015,15 @@ TEST_P( // Testing that values are read from state 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) +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 - for (auto i = 0u; i < 3; ++i) { + for (auto i = 0u; i < 3; ++i) + { joint_pos_[i] = 3.1 + i; joint_vel_[i] = 0.25 + i; joint_acc_[i] = 0.02 + i / 10.0; @@ -1044,11 +1032,13 @@ TEST_P( auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - for (auto i = 0u; i < 3; ++i) { + for (auto i = 0u; i < 3; ++i) + { EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); // check velocity - if (std::find( + if ( + std::find( state_interface_types_.begin(), state_interface_types_.end(), hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && std::find( @@ -1059,7 +1049,8 @@ TEST_P( } // check acceleration - if (std::find( + if ( + std::find( state_interface_types_.begin(), state_interface_types_.end(), hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && std::find( @@ -1073,47 +1064,35 @@ TEST_P( executor.cancel(); } - // TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P // position controllers INSTANTIATE_TEST_CASE_P( - PositionTrajectoryControllers, - TrajectoryControllerTestParameterized, + PositionTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), std::make_tuple( - std::vector({"position"}), - std::vector({"position"})), - std::make_tuple( - std::vector({"position"}), - std::vector({"position", "velocity"})), + std::vector({"position"}), std::vector({"position", "velocity"})), std::make_tuple( std::vector({"position"}), - std::vector({"position", "velocity", "acceleration"})) - ) -); + std::vector({"position", "velocity", "acceleration"})))); // position_velocity controllers INSTANTIATE_TEST_CASE_P( - PositionVelocityTrajectoryControllers, - TrajectoryControllerTestParameterized, + PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( - std::vector({"position", "velocity"}), - std::vector({"position"})), + std::vector({"position", "velocity"}), std::vector({"position"})), std::make_tuple( std::vector({"position", "velocity"}), std::vector({"position", "velocity"})), std::make_tuple( std::vector({"position", "velocity"}), - std::vector({"position", "velocity", "acceleration"})) - ) -); + std::vector({"position", "velocity", "acceleration"})))); // position_velocity_acceleration controllers INSTANTIATE_TEST_CASE_P( - PositionVelocityAccelerationTrajectoryControllers, - TrajectoryControllerTestParameterized, + PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( std::vector({"position", "velocity", "acceleration"}), @@ -1123,17 +1102,16 @@ INSTANTIATE_TEST_CASE_P( std::vector({"position", "velocity"})), std::make_tuple( std::vector({"position", "velocity", "acceleration"}), - std::vector({"position", "velocity", "acceleration"})) - ) -); + std::vector({"position", "velocity", "acceleration"})))); -TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ auto set_parameter_and_check_result = [&]() { - EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - SetParameters(); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - }; + EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + SetParameters(); // This call is replacing the way parameters are set via launch + traj_controller_->configure(); + EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + }; SetUpTrajectoryController(false); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 1b5f20dcc8..5224a64aeb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -40,20 +40,19 @@ const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; namespace test_trajectory_controllers { - -class TestableJointTrajectoryController : public joint_trajectory_controller:: - JointTrajectoryController +class TestableJointTrajectoryController +: public joint_trajectory_controller::JointTrajectoryController { public: - using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController; + using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = - joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); + auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); // this class can still be useful without the wait set - if (joint_command_subscriber_) { + if (joint_command_subscriber_) + { joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); } return ret; @@ -68,19 +67,17 @@ class TestableJointTrajectoryController : public joint_trajectory_controller:: */ bool wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds {500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) { + if (success) + { executor.spin_some(); } return success; } - void set_joint_names(const std::vector & joint_names) - { - joint_names_ = joint_names; - } + void set_joint_names(const std::vector & joint_names) { joint_names_ = joint_names; } void set_command_interfaces(const std::vector & command_interfaces) { @@ -103,10 +100,7 @@ class TestableJointTrajectoryController : public joint_trajectory_controller:: class TrajectoryControllerTest : public ::testing::Test { public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } virtual void SetUp() { @@ -131,13 +125,15 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController(bool use_local_parameters = true) { traj_controller_ = std::make_shared(); - if (use_local_parameters) { + if (use_local_parameters) + { traj_controller_->set_joint_names(joint_names_); traj_controller_->set_command_interfaces(command_interface_types_); traj_controller_->set_state_interfaces(state_interface_types_); } auto ret = traj_controller_->init(controller_name_); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { FAIL(); } } @@ -152,18 +148,18 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpAndActivateTrajectoryController( - bool use_local_parameters = true, - const std::vector & parameters = {}, - rclcpp::Executor * executor = nullptr, - bool separate_cmd_and_state_values = false) + bool use_local_parameters = true, const std::vector & parameters = {}, + rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); traj_node_ = traj_controller_->get_node(); - for (const auto & param : parameters) { + for (const auto & param : parameters) + { traj_node_->set_parameter(param); } - if (executor) { + if (executor) + { executor->add_node(traj_node_->get_node_base_interface()); } @@ -185,35 +181,24 @@ class TrajectoryControllerTest : public ::testing::Test pos_state_interfaces_.reserve(joint_names_.size()); vel_state_interfaces_.reserve(joint_names_.size()); acc_state_interfaces_.reserve(joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) { - pos_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( - joint_names_[i], - hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( - joint_names_[i], - hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back( - hardware_interface::CommandInterface( - joint_names_[i], - hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - - pos_state_interfaces_.emplace_back( - hardware_interface::StateInterface( - joint_names_[i], - hardware_interface::HW_IF_POSITION, - separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); - vel_state_interfaces_.emplace_back( - hardware_interface::StateInterface( - joint_names_[i], - hardware_interface::HW_IF_VELOCITY, - separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); - acc_state_interfaces_.emplace_back( - hardware_interface::StateInterface( - joint_names_[i], - hardware_interface::HW_IF_ACCELERATION, - separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); + vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); + acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + + pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); + vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); + acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_ACCELERATION, + separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); @@ -234,18 +219,13 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->activate(); } - static void TearDownTestCase() - { - rclcpp::shutdown(); - } + static void TearDownTestCase() { rclcpp::shutdown(); } void subscribeToState() { auto traj_lifecycle_node = traj_controller_->get_node(); traj_lifecycle_node->set_parameter( - rclcpp::Parameter( - "state_publish_rate", - static_cast(100))); + rclcpp::Parameter("state_publish_rate", static_cast(100))); using control_msgs::msg::JointTrajectoryControllerState; @@ -253,15 +233,11 @@ class TrajectoryControllerTest : public ::testing::Test // Needed, otherwise spin_some() returns only the oldest message in the queue // I do not understand why spin_some provides only one message qos.keep_last(1); - state_subscriber_ = - traj_lifecycle_node->create_subscription( - controller_name_ + "/state", - qos, - [&](std::shared_ptr msg) { + state_subscriber_ = traj_lifecycle_node->create_subscription( + controller_name_ + "/state", qos, [&](std::shared_ptr msg) { std::lock_guard guard(state_mutex_); state_msg_ = msg; - } - ); + }); } /// Publish trajectory msgs with multiple points @@ -272,16 +248,16 @@ class TrajectoryControllerTest : public ::testing::Test */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, - rclcpp::Time start_time = rclcpp::Time(), + const std::vector> & points, rclcpp::Time start_time = rclcpp::Time(), const std::vector & joint_names = {}) { int wait_count = 0; const auto topic = trajectory_publisher_->get_topic_name(); - while (node_->count_subscribers(topic) == 0) { - if (wait_count >= 5) { - auto error_msg = - std::string("publishing to ") + topic + " but no node subscribes to it"; + while (node_->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; throw std::runtime_error(error_msg); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -289,9 +265,12 @@ class TrajectoryControllerTest : public ::testing::Test } trajectory_msgs::msg::JointTrajectory traj_msg; - if (joint_names.empty()) { + if (joint_names.empty()) + { traj_msg.joint_names = {joint_names_.begin(), joint_names_.begin() + points[0].size()}; - } else { + } + else + { traj_msg.joint_names = joint_names; } traj_msg.header.stamp = start_time; @@ -303,11 +282,13 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Duration duration(duration_msg); rclcpp::Duration duration_total(duration_msg); - for (size_t index = 0; index < points.size(); ++index) { + for (size_t index = 0; index < points.size(); ++index) + { traj_msg.points[index].time_from_start = duration_total; traj_msg.points[index].positions.resize(points[index].size()); - for (size_t j = 0; j < points[index].size(); ++j) { + for (size_t j = 0; j < points[index].size(); ++j) + { traj_msg.points[index].positions[j] = points[index][j]; } duration_total = duration_total + duration; @@ -320,15 +301,16 @@ class TrajectoryControllerTest : public ::testing::Test { const auto start_time = rclcpp::Clock().now(); const auto end_time = start_time + wait_time; - while (rclcpp::Clock().now() < end_time) { + while (rclcpp::Clock().now() < end_time) + { traj_controller_->update(); } } void waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, - trajectory_msgs::msg::JointTrajectoryPoint expected_desired, - rclcpp::Executor & executor, rclcpp::Duration controller_wait_time, double allowed_delta) + trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, + rclcpp::Duration controller_wait_time, double allowed_delta) { { std::lock_guard guard(state_mutex_); @@ -340,13 +322,15 @@ class TrajectoryControllerTest : public ::testing::Test executor.spin_some(); auto state_msg = getState(); ASSERT_TRUE(state_msg); - for (size_t i = 0; i < expected_actual.positions.size(); ++i) { + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); } - for (size_t i = 0; i < expected_desired.positions.size(); ++i) { + for (size_t i = 0; i < expected_desired.positions.size(); ++i) + { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); @@ -392,9 +376,9 @@ class TrajectoryControllerTest : public ::testing::Test // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class TrajectoryControllerTestParameterized - : public TrajectoryControllerTest, - public ::testing::WithParamInterface, std::vector>> +: public TrajectoryControllerTest, + public ::testing::WithParamInterface< + std::tuple, std::vector>> { public: virtual void SetUp() @@ -404,10 +388,7 @@ class TrajectoryControllerTestParameterized state_interface_types_ = std::get<1>(GetParam()); } - static void TearDownTestCase() - { - TrajectoryControllerTest::TearDownTestCase(); - } + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; } // namespace test_trajectory_controllers diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 44ea025765..98beea35e4 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -48,12 +48,9 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_joint_group_position_controller test/test_load_joint_group_position_controller.cpp diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index f5d91dd6b2..2051ae44ab 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -22,7 +22,6 @@ namespace position_controllers { - /** * \brief Forward command controller for a set of position controlled joints (linear or angular). * @@ -39,8 +38,8 @@ class JointGroupPositionController : public forward_command_controller::ForwardC POSITION_CONTROLLERS_PUBLIC JointGroupPositionController(); - POSITION_CONTROLLERS_PUBLIC controller_interface::return_type - init(const std::string & controller_name) override; + POSITION_CONTROLLERS_PUBLIC controller_interface::return_type init( + const std::string & controller_name) override; }; } // namespace position_controllers diff --git a/position_controllers/include/position_controllers/visibility_control.h b/position_controllers/include/position_controllers/visibility_control.h index b989be0461..cac7aebc5a 100644 --- a/position_controllers/include/position_controllers/visibility_control.h +++ b/position_controllers/include/position_controllers/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define POSITION_CONTROLLERS_EXPORT __attribute__ ((dllexport)) - #define POSITION_CONTROLLERS_IMPORT __attribute__ ((dllimport)) - #else - #define POSITION_CONTROLLERS_EXPORT __declspec(dllexport) - #define POSITION_CONTROLLERS_IMPORT __declspec(dllimport) - #endif - #ifdef POSITION_CONTROLLERS_BUILDING_DLL - #define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_EXPORT - #else - #define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_IMPORT - #endif - #define POSITION_CONTROLLERS_PUBLIC_TYPE POSITION_CONTROLLERS_PUBLIC - #define POSITION_CONTROLLERS_LOCAL +#ifdef __GNUC__ +#define POSITION_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define POSITION_CONTROLLERS_IMPORT __attribute__((dllimport)) #else - #define POSITION_CONTROLLERS_EXPORT __attribute__ ((visibility("default"))) - #define POSITION_CONTROLLERS_IMPORT - #if __GNUC__ >= 4 - #define POSITION_CONTROLLERS_PUBLIC __attribute__ ((visibility("default"))) - #define POSITION_CONTROLLERS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define POSITION_CONTROLLERS_PUBLIC - #define POSITION_CONTROLLERS_LOCAL - #endif - #define POSITION_CONTROLLERS_PUBLIC_TYPE +#define POSITION_CONTROLLERS_EXPORT __declspec(dllexport) +#define POSITION_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef POSITION_CONTROLLERS_BUILDING_DLL +#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_EXPORT +#else +#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_IMPORT +#endif +#define POSITION_CONTROLLERS_PUBLIC_TYPE POSITION_CONTROLLERS_PUBLIC +#define POSITION_CONTROLLERS_LOCAL +#else +#define POSITION_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define POSITION_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define POSITION_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define POSITION_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSITION_CONTROLLERS_PUBLIC +#define POSITION_CONTROLLERS_LOCAL +#endif +#define POSITION_CONTROLLERS_PUBLIC_TYPE #endif #endif // POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8d69542495..e3211ea7f2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -16,8 +16,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager hardware_interface ros2_control_test_assets diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index aef20ff8ce..acbe4b62c3 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -30,20 +30,24 @@ JointGroupPositionController::JointGroupPositionController() interface_name_ = hardware_interface::HW_IF_POSITION; } -controller_interface::return_type -JointGroupPositionController::init(const std::string & controller_name) +controller_interface::return_type JointGroupPositionController::init( + const std::string & controller_name) { auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupPositionController constructor. get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_POSITION)); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 21f5ea8d58..f07561d4fb 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -39,27 +39,18 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription const auto timeout = std::chrono::seconds(10); return wait_set.wait(timeout).kind(); } -} +} // namespace -void JointGroupPositionControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void JointGroupPositionControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } void JointGroupPositionControllerTest::SetUp() { controller_ = std::make_unique(); } -void JointGroupPositionControllerTest::TearDown() -{ - controller_.reset(nullptr); -} +void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupPositionControllerTest::SetUpController() { @@ -131,8 +122,7 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -152,8 +142,7 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -200,8 +189,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string( - controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); std_msgs::msg::Float64MultiArray command_msg; command_msg.data = {10.0, 20.0, 30.0}; command_pub->publish(command_msg); diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 773c3d6af3..93149d8e19 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -25,8 +25,8 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" -using hardware_interface::HW_IF_POSITION; using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index de52f7eed6..cc376bd2d4 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -29,12 +29,11 @@ TEST(TestLoadJointGroupPositionController, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_group_position_controller", - "position_controllers/JointGroupPositionController")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_group_position_controller", "position_controllers/JointGroupPositionController")); rclcpp::shutdown(); } diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index e8e6c536d0..6107486f91 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -47,12 +47,9 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_lint_auto_find_test_dependencies() - ament_add_gmock( test_load_joint_group_velocity_controller test/test_load_joint_group_velocity_controller.cpp diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index 81508b36f3..6e7ff3d26f 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -22,7 +22,6 @@ namespace velocity_controllers { - /** * \brief Forward command controller for a set of velocity controlled joints (linear or angular). * @@ -40,12 +39,10 @@ class JointGroupVelocityController : public forward_command_controller::ForwardC JointGroupVelocityController(); VELOCITY_CONTROLLERS_PUBLIC - controller_interface::return_type - init(const std::string & controller_name) override; + controller_interface::return_type init(const std::string & controller_name) override; VELOCITY_CONTROLLERS_PUBLIC - CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; }; } // namespace velocity_controllers diff --git a/velocity_controllers/include/velocity_controllers/visibility_control.h b/velocity_controllers/include/velocity_controllers/visibility_control.h index 5293bc6710..08b4ecc588 100644 --- a/velocity_controllers/include/velocity_controllers/visibility_control.h +++ b/velocity_controllers/include/velocity_controllers/visibility_control.h @@ -26,31 +26,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define VELOCITY_CONTROLLERS_EXPORT __attribute__ ((dllexport)) - #define VELOCITY_CONTROLLERS_IMPORT __attribute__ ((dllimport)) - #else - #define VELOCITY_CONTROLLERS_EXPORT __declspec(dllexport) - #define VELOCITY_CONTROLLERS_IMPORT __declspec(dllimport) - #endif - #ifdef VELOCITY_CONTROLLERS_BUILDING_DLL - #define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT - #else - #define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT - #endif - #define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC - #define VELOCITY_CONTROLLERS_LOCAL +#ifdef __GNUC__ +#define VELOCITY_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define VELOCITY_CONTROLLERS_IMPORT __attribute__((dllimport)) #else - #define VELOCITY_CONTROLLERS_EXPORT __attribute__ ((visibility("default"))) - #define VELOCITY_CONTROLLERS_IMPORT - #if __GNUC__ >= 4 - #define VELOCITY_CONTROLLERS_PUBLIC __attribute__ ((visibility("default"))) - #define VELOCITY_CONTROLLERS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define VELOCITY_CONTROLLERS_PUBLIC - #define VELOCITY_CONTROLLERS_LOCAL - #endif - #define VELOCITY_CONTROLLERS_PUBLIC_TYPE +#define VELOCITY_CONTROLLERS_EXPORT __declspec(dllexport) +#define VELOCITY_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef VELOCITY_CONTROLLERS_BUILDING_DLL +#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT +#else +#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT +#endif +#define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC +#define VELOCITY_CONTROLLERS_LOCAL +#else +#define VELOCITY_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define VELOCITY_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define VELOCITY_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define VELOCITY_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define VELOCITY_CONTROLLERS_PUBLIC +#define VELOCITY_CONTROLLERS_LOCAL +#endif +#define VELOCITY_CONTROLLERS_PUBLIC_TYPE #endif #endif // VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index dab0f19200..1aae1c4f83 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -16,8 +16,6 @@ pluginlib ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 0366b279ef..b349b4ef45 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -15,9 +15,9 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "velocity_controllers/joint_group_velocity_controller.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" +#include "velocity_controllers/joint_group_velocity_controller.hpp" namespace velocity_controllers { @@ -30,21 +30,24 @@ JointGroupVelocityController::JointGroupVelocityController() interface_name_ = hardware_interface::HW_IF_VELOCITY; } -controller_interface::return_type -JointGroupVelocityController::init( +controller_interface::return_type JointGroupVelocityController::init( const std::string & controller_name) { auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) { + if (ret != controller_interface::return_type::OK) + { return ret; } - try { + try + { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupVelocityController constructor. get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_VELOCITY)); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } @@ -58,7 +61,8 @@ CallbackReturn JointGroupVelocityController::on_deactivate( auto ret = ForwardCommandController::on_deactivate(previous_state); // stop all joints - for (auto & command_interface : command_interfaces_) { + for (auto & command_interface : command_interfaces_) + { command_interface.set_value(0.0); } diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index a3708c6e43..c0dce9b4bf 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -39,27 +39,18 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription const auto timeout = std::chrono::seconds(10); return wait_set.wait(timeout).kind(); } -} +} // namespace -void JointGroupVelocityControllerTest::SetUpTestCase() -{ - rclcpp::init(0, nullptr); -} +void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void JointGroupVelocityControllerTest::TearDownTestCase() -{ - rclcpp::shutdown(); -} +void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } void JointGroupVelocityControllerTest::SetUp() { controller_ = std::make_unique(); } -void JointGroupVelocityControllerTest::TearDown() -{ - controller_.reset(nullptr); -} +void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupVelocityControllerTest::SetUpController() { @@ -131,8 +122,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -152,8 +142,7 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = - std::make_shared(); + auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); @@ -200,8 +189,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string( - controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); std_msgs::msg::Float64MultiArray command_msg; command_msg.data = {10.0, 20.0, 30.0}; command_pub->publish(command_msg); @@ -221,7 +209,6 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } - TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) { SetUpController(); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 5b21ce8eae..e94f7f082d 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -25,9 +25,8 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" -using hardware_interface::HW_IF_VELOCITY; using hardware_interface::CommandInterface; - +using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 23f42f2ecc..36d5a8368d 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -29,12 +29,11 @@ TEST(TestLoadJointGroupVelocityController, load_controller) controller_manager::ControllerManager cm( std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_joint_group_velocity_controller", - "velocity_controllers/JointGroupVelocityController")); + ASSERT_NO_THROW(cm.load_controller( + "test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController")); rclcpp::shutdown(); } From 413a663e6a7e75a93cd4341ec2e08e35744df4a3 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Mon, 30 Aug 2021 08:57:14 +0200 Subject: [PATCH 011/524] Add auto declaration of parameters. (#224) --- .../src/diff_drive_controller.cpp | 89 +++++++++---------- .../src/force_torque_sensor_broadcaster.cpp | 16 ++-- .../src/forward_command_controller.cpp | 5 +- .../gripper_action_controller_impl.hpp | 12 +-- .../hardware_interface_adapter.hpp | 23 ++++- .../src/imu_sensor_broadcaster.cpp | 4 +- .../src/joint_state_broadcaster.cpp | 2 +- .../src/joint_trajectory_controller.cpp | 19 ++-- 8 files changed, 88 insertions(+), 82 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 46f074b852..f86b08c196 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -59,55 +59,48 @@ controller_interface::return_type DiffDriveController::init(const std::string & try { - auto node = get_node(); // with the lifecycle node being initialized, we can declare parameters - node->declare_parameter>( - "left_wheel_names", std::vector()); - node->declare_parameter>( - "right_wheel_names", std::vector()); - - node->declare_parameter("wheel_separation", wheel_params_.separation); - node->declare_parameter("wheels_per_side", wheel_params_.wheels_per_side); - node->declare_parameter("wheel_radius", wheel_params_.radius); - node->declare_parameter( - "wheel_separation_multiplier", wheel_params_.separation_multiplier); - node->declare_parameter( - "left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); - node->declare_parameter( - "right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); - - node->declare_parameter("odom_frame_id", odom_params_.odom_frame_id); - node->declare_parameter("base_frame_id", odom_params_.base_frame_id); - node->declare_parameter>("pose_covariance_diagonal", std::vector()); - node->declare_parameter>( - "twist_covariance_diagonal", std::vector()); - node->declare_parameter("open_loop", odom_params_.open_loop); - node->declare_parameter("enable_odom_tf", odom_params_.enable_odom_tf); - - node->declare_parameter("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - node->declare_parameter("publish_limited_velocity", publish_limited_velocity_); - node->declare_parameter("velocity_rolling_window_size", 10); - node->declare_parameter("use_stamped_vel", use_stamped_vel_); - - node->declare_parameter("linear.x.has_velocity_limits", false); - node->declare_parameter("linear.x.has_acceleration_limits", false); - node->declare_parameter("linear.x.has_jerk_limits", false); - node->declare_parameter("linear.x.max_velocity", NAN); - node->declare_parameter("linear.x.min_velocity", NAN); - node->declare_parameter("linear.x.max_acceleration", NAN); - node->declare_parameter("linear.x.min_acceleration", NAN); - node->declare_parameter("linear.x.max_jerk", NAN); - node->declare_parameter("linear.x.min_jerk", NAN); - - node->declare_parameter("angular.z.has_velocity_limits", false); - node->declare_parameter("angular.z.has_acceleration_limits", false); - node->declare_parameter("angular.z.has_jerk_limits", false); - node->declare_parameter("angular.z.max_velocity", NAN); - node->declare_parameter("angular.z.min_velocity", NAN); - node->declare_parameter("angular.z.max_acceleration", NAN); - node->declare_parameter("angular.z.min_acceleration", NAN); - node->declare_parameter("angular.z.max_jerk", NAN); - node->declare_parameter("angular.z.min_jerk", NAN); + auto_declare>("left_wheel_names", std::vector()); + auto_declare>("right_wheel_names", std::vector()); + + auto_declare("wheel_separation", wheel_params_.separation); + auto_declare("wheels_per_side", wheel_params_.wheels_per_side); + auto_declare("wheel_radius", wheel_params_.radius); + auto_declare("wheel_separation_multiplier", wheel_params_.separation_multiplier); + auto_declare("left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); + auto_declare("right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + auto_declare("publish_limited_velocity", publish_limited_velocity_); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + + auto_declare("linear.x.has_velocity_limits", false); + auto_declare("linear.x.has_acceleration_limits", false); + auto_declare("linear.x.has_jerk_limits", false); + auto_declare("linear.x.max_velocity", NAN); + auto_declare("linear.x.min_velocity", NAN); + auto_declare("linear.x.max_acceleration", NAN); + auto_declare("linear.x.min_acceleration", NAN); + auto_declare("linear.x.max_jerk", NAN); + auto_declare("linear.x.min_jerk", NAN); + + auto_declare("angular.z.has_velocity_limits", false); + auto_declare("angular.z.has_acceleration_limits", false); + auto_declare("angular.z.has_jerk_limits", false); + auto_declare("angular.z.max_velocity", NAN); + auto_declare("angular.z.min_velocity", NAN); + auto_declare("angular.z.max_acceleration", NAN); + auto_declare("angular.z.min_acceleration", NAN); + auto_declare("angular.z.max_jerk", NAN); + auto_declare("angular.z.min_jerk", NAN); } catch (const std::exception & e) { diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index cbc19cfa3a..3a928f90e6 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -39,14 +39,14 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::init( try { - node_->declare_parameter("sensor_name", ""); - node_->declare_parameter("interface_names.force.x", ""); - node_->declare_parameter("interface_names.force.y", ""); - node_->declare_parameter("interface_names.force.z", ""); - node_->declare_parameter("interface_names.torque.x", ""); - node_->declare_parameter("interface_names.torque.y", ""); - node_->declare_parameter("interface_names.torque.z", ""); - node_->declare_parameter("frame_id", ""); + auto_declare("sensor_name", ""); + auto_declare("interface_names.force.x", ""); + auto_declare("interface_names.force.y", ""); + auto_declare("interface_names.force.z", ""); + auto_declare("interface_names.torque.x", ""); + auto_declare("interface_names.torque.y", ""); + auto_declare("interface_names.torque.z", ""); + auto_declare("frame_id", ""); } catch (const std::exception & e) { diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index c4bf8fcccb..1c800d2ee7 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -46,10 +46,9 @@ controller_interface::return_type ForwardCommandController::init( try { - auto node = get_node(); - node->declare_parameter>("joints", std::vector()); + auto_declare>("joints", std::vector()); - node->declare_parameter("interface_name", ""); + auto_declare("interface_name", ""); } catch (const std::exception & e) { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 96cf9465b9..4f3a994b95 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -46,12 +46,12 @@ controller_interface::return_type GripperActionController::in } // with the lifecycle node being initialized, we can declare parameters - node_->declare_parameter("action_monitor_rate", 20.0); - node_->declare_parameter("joint", joint_name_); - node_->declare_parameter("goal_tolerance", 0.01); - node_->declare_parameter("max_effort", 0.0); - node_->declare_parameter("stall_velocity_threshold", 0.001); - node_->declare_parameter("stall_timeout", 1.0); + auto_declare("action_monitor_rate", 20.0); + auto_declare("joint", joint_name_); + auto_declare("goal_tolerance", 0.01); + auto_declare("max_effort", 0.0); + auto_declare("stall_velocity_threshold", 0.001); + auto_declare("stall_timeout", 1.0); return controller_interface::return_type::OK; } diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 1e1a6a2c8c..cbf38e4e0a 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -114,6 +114,21 @@ template <> class HardwareInterfaceAdapter { public: + template + auto auto_declare( + const rclcpp::Node::SharedPtr & node, const std::string & name, + const ParameterT & default_value) + { + if (!node->has_parameter(name)) + { + return node->declare_parameter(name, default_value); + } + else + { + return node->get_parameter(name).get_value(); + } + } + bool init( std::experimental::optional> joint_handle, @@ -122,10 +137,10 @@ class HardwareInterfaceAdapter joint_handle_ = joint_handle; // Init PID gains from ROS parameter server const std::string prefix = "gains." + joint_handle_->get().get_name(); - const auto k_p = node->declare_parameter(prefix + ".p", 0.0); - const auto k_i = node->declare_parameter(prefix + ".i", 0.0); - const auto k_d = node->declare_parameter(prefix + ".d", 0.0); - const auto i_clamp = node->declare_parameter(prefix + ".i_clamp", 0.0); + const auto k_p = auto_declare(node, prefix + ".p", 0.0); + const auto k_i = auto_declare(node, prefix + ".i", 0.0); + const auto k_d = auto_declare(node, prefix + ".d", 0.0); + const auto i_clamp = auto_declare(node, prefix + ".i_clamp", 0.0); // Initialize PID pid_ = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); return true; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 34cc3b9d76..8ada847685 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -33,8 +33,8 @@ controller_interface::return_type IMUSensorBroadcaster::init(const std::string & try { - node_->declare_parameter("sensor_name", ""); - node_->declare_parameter("frame_id", ""); + auto_declare("sensor_name", ""); + auto_declare("frame_id", ""); } catch (const std::exception & e) { diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index f17df64654..59c9a73cb1 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -56,7 +56,7 @@ controller_interface::return_type JointStateBroadcaster::init(const std::string try { - get_node()->declare_parameter("use_local_topics", false); + auto_declare("use_local_topics", false); } catch (const std::exception & e) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7e39cba1be..37a93e0709 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -60,16 +60,15 @@ controller_interface::return_type JointTrajectoryController::init( } // with the lifecycle node being initialized, we can declare parameters - node_->declare_parameter>("joints", joint_names_); - node_->declare_parameter>( - "command_interfaces", command_interface_types_); - node_->declare_parameter>("state_interfaces", state_interface_types_); - node_->declare_parameter("state_publish_rate", 50.0); - node_->declare_parameter("action_monitor_rate", 20.0); - node_->declare_parameter("allow_partial_joints_goal", allow_partial_joints_goal_); - node_->declare_parameter("open_loop_control", open_loop_control_); - node_->declare_parameter("constraints.stopped_velocity_tolerance", 0.01); - node_->declare_parameter("constraints.goal_time", 0.0); + auto_declare>("joints", joint_names_); + auto_declare>("command_interfaces", command_interface_types_); + auto_declare>("state_interfaces", state_interface_types_); + auto_declare("state_publish_rate", 50.0); + auto_declare("action_monitor_rate", 20.0); + auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); + auto_declare("open_loop_control", open_loop_control_); + auto_declare("constraints.stopped_velocity_tolerance", 0.01); + auto_declare("constraints.goal_time", 0.0); return controller_interface::return_type::OK; } From 79743605b4422e4167d37573edfbbabf2fb5e740 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 30 Aug 2021 08:20:43 +0100 Subject: [PATCH 012/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 8 ++++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ forward_command_controller/CHANGELOG.rst | 8 ++++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ joint_state_broadcaster/CHANGELOG.rst | 9 +++++++++ joint_state_controller/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 12 ++++++++++++ position_controllers/CHANGELOG.rst | 8 ++++++++ ros2_controllers/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 8 ++++++++ 12 files changed, 91 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5538bc0db2..1f670f5034 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6baa8894d5..8665bbf070 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak + 0.4.1 (2021-07-08) ------------------ * Fix test dependencies (`#213 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 497ba92601..ff6342d7e6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Contributors: Bence Magyar, Denis Štogl, livanov93 + 0.4.1 (2021-07-08) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index eb1fc95a17..0fa9a7cfb8 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e554f53410..36d52ece4d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Contributors: Bence Magyar, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ * Fix test dependencies (`#213 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 66cf200abd..1dde0010c0 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ba970b1a76..e1324cd81c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* [Joint State Broadcaster] Add option to publish joint states to local topics (`#218 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst index 2c0f757440..4220f414a9 100644 --- a/joint_state_controller/CHANGELOG.rst +++ b/joint_state_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Contributors: Bence Magyar + 0.4.1 (2021-07-08) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 97ff2f79a1..1f55a5b1d9 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Enable JTC for hardware having offset from state measurements (`#189 `_) + * Avoid "jumps" with states that have tracking error. All test are passing but separatelly. Is there some kind of timeout? + * Remove allow_integration_flag + * Add reading from command interfaces when restarting controller +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + 0.4.1 (2021-07-08) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 88f35f207a..a25435c331 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak + 0.4.1 (2021-07-08) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 9b4bb47adc..44a80706bf 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add initial pre-commit setup. (`#220 `_) +* Contributors: Denis Štogl + 0.4.1 (2021-07-08) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8c682ef5f5..84a8237c12 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak + 0.4.1 (2021-07-08) ------------------ From beee606b90fe11ea703b00affdf01b01e8527137 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 30 Aug 2021 08:21:04 +0100 Subject: [PATCH 013/524] 0.5.0 --- 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_state_controller/CHANGELOG.rst | 4 ++-- joint_state_controller/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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1f670f5034..d6690cab9d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index f2f622084d..9e1207679d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 0.4.1 + 0.5.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 8665bbf070..35429e42a2 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) * Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e9ca03a1f3..786c55681b 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 0.4.1 + 0.5.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 ff6342d7e6..8ab8021ba1 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 ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 901eb559cb..0daf5d5f40 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 0.4.1 + 0.5.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 0fa9a7cfb8..098499a771 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 9c0294516c..8d2aa4bf22 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 0.4.1 + 0.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 36d52ece4d..a827b6898a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Contributors: Bence Magyar, Lovro Ivanov diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index da568d0f45..accb5ab0c5 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 0.4.1 + 0.5.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1dde0010c0..ba443fc8fa 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index aeb3efaa4c..83b38f0b56 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 0.4.1 + 0.5.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 e1324cd81c..65ab8f1f69 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * [Joint State Broadcaster] Add option to publish joint states to local topics (`#218 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fdcd281737..155e4526ed 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 0.4.1 + 0.5.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst index 4220f414a9..c59c32de4e 100644 --- a/joint_state_controller/CHANGELOG.rst +++ b/joint_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) * Contributors: Bence Magyar diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml index 9d149bfccc..3cdc992ec2 100644 --- a/joint_state_controller/package.xml +++ b/joint_state_controller/package.xml @@ -1,7 +1,7 @@ joint_state_controller - 0.4.1 + 0.5.0 Controller to publish joint state Bence Magyar Jordan Palacios diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1f55a5b1d9..ddf376b1bc 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add auto declaration of parameters. (`#224 `_) * Bring precommit config up to speed with ros2_control (`#227 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 133f832cf8..e55ce46020 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 0.4.1 + 0.5.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 a25435c331..31a74e0ca3 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) * Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e3211ea7f2..f990c94e57 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 0.4.1 + 0.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 44a80706bf..d3d9681757 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Add initial pre-commit setup. (`#220 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 9f15523277..576f29b80f 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 0.4.1 + 0.5.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 84a8237c12..7e3aee7ac1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2021-08-30) +------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) * Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) * Add initial pre-commit setup. (`#220 `_) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 1aae1c4f83..8656057ddd 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 0.4.1 + 0.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From b7947682d6c51889033aaa6e24744bfe7a54a31f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 31 Aug 2021 08:51:04 +0100 Subject: [PATCH 014/524] Remove joint_state_controller, use joint_state_broadcaster instead (#230) * Remove joint_state_controller, use joint_state_broadcaster instead * remove from metapackage --- joint_state_controller/CHANGELOG.rst | 56 ---------------- joint_state_controller/CMakeLists.txt | 65 ------------------- joint_state_controller/doc/userdoc.rst | 12 ---- joint_state_controller/joint_state_plugin.xml | 16 ----- joint_state_controller/package.xml | 33 ---------- .../src/joint_state_controller.cpp | 19 ------ .../test/test_load_joint_state_controller.cpp | 43 ------------ ros2_controllers/package.xml | 1 - 8 files changed, 245 deletions(-) delete mode 100644 joint_state_controller/CHANGELOG.rst delete mode 100644 joint_state_controller/CMakeLists.txt delete mode 100644 joint_state_controller/doc/userdoc.rst delete mode 100644 joint_state_controller/joint_state_plugin.xml delete mode 100644 joint_state_controller/package.xml delete mode 100644 joint_state_controller/src/joint_state_controller.cpp delete mode 100644 joint_state_controller/test/test_load_joint_state_controller.cpp diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst deleted file mode 100644 index c59c32de4e..0000000000 --- a/joint_state_controller/CHANGELOG.rst +++ /dev/null @@ -1,56 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package joint_state_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Contributors: Bence Magyar - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Rename joint_state_controller -> joint_state_broadcaster (`#160 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar, Matt Reynolds - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* avoid warnings (`#137 `_) -* Contributors: Karsten Knese - -0.1.0 (2020-12-23) ------------------- -* Print exception msg (`#130 `_) -* Remove lifecycle node controllers (`#124 `_) -* joint state controller with resource manager (`#109 `_) -* Use new joint handles in all controllers (`#90 `_) -* Clock() defaults to RCL_SYSTEM_TIME without considering parameter use_sim_time. (`#83 `_) -* Joint states are now published using the DynamicJointState message (`#65 `_) -* Add on_activate to JointStateController (`#43 `_) -* Contributors: Alejandro Hernández Cordero, Bence Magyar, Denis Štogl, Hang, Jordan Palacios, Karsten Knese, Victor Lopez, Yutaka Kondo diff --git a/joint_state_controller/CMakeLists.txt b/joint_state_controller/CMakeLists.txt deleted file mode 100644 index 669460a940..0000000000 --- a/joint_state_controller/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_state_controller) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(joint_state_broadcaster REQUIRED) -find_package(pluginlib REQUIRED) - -add_library(joint_state_controller - SHARED - src/joint_state_controller.cpp -) -ament_target_dependencies(joint_state_controller - joint_state_broadcaster -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_state_controller PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(joint_state_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) - -install( - TARGETS - joint_state_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) - find_package(rclcpp REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock( - test_load_joint_state_controller - test/test_load_joint_state_controller.cpp - ) - target_include_directories(test_load_joint_state_controller PRIVATE include) - ament_target_dependencies(test_load_joint_state_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - target_compile_definitions(test_load_joint_state_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -endif() - -ament_export_dependencies( - joint_state_broadcaster -) -ament_export_libraries( - joint_state_controller -) -ament_package() diff --git a/joint_state_controller/doc/userdoc.rst b/joint_state_controller/doc/userdoc.rst deleted file mode 100644 index 247920ee2c..0000000000 --- a/joint_state_controller/doc/userdoc.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _joint_state_controller_userdoc: - -joint_state_controller ----------------------- - -The ``joint_state_controller`` has been renamed to ``joint_state_broadcaster`` -in order to differentiate between true controllers and simple data broadcasters. - -This package will be removed in ROS H-Turtle. Instead, use package -``joint_state_broadcaster``. - -Documentation for ``joint_state_broadcaster`` can be found `here `_. diff --git a/joint_state_controller/joint_state_plugin.xml b/joint_state_controller/joint_state_plugin.xml deleted file mode 100644 index 463c0ff0c6..0000000000 --- a/joint_state_controller/joint_state_plugin.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - The joint state controller publishes the current joint state - - ---------------------------------------------- - - Warning: joint_state_controller is deprecated! - - This package has been renamed to joint_state_broadcaster in order to - differentiate between true controllers and simple data broadcasters. - This package will be removed in ROS H-Turtle. Instead, use package - joint_state_broadcaster. - - - diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml deleted file mode 100644 index 3cdc992ec2..0000000000 --- a/joint_state_controller/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - joint_state_controller - 0.5.0 - Controller to publish joint state - Bence Magyar - Jordan Palacios - Karsten Knese - - Apache License 2.0 - - ament_cmake - - joint_state_broadcaster - - pluginlib - - ament_cmake_gmock - controller_manager - hardware_interface - rclcpp - ros2_control_test_assets - - - ament_cmake - - This package has been renamed to joint_state_broadcaster in order to - differentiate between true controllers and simple data broadcasters. - This package will be removed in ROS H-Turtle. Instead, use package - joint_state_broadcaster. - - - diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp deleted file mode 100644 index 3f369d2214..0000000000 --- a/joint_state_controller/src/joint_state_controller.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "joint_state_broadcaster/joint_state_broadcaster.hpp" -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - joint_state_broadcaster::JointStateBroadcaster, controller_interface::ControllerInterface) diff --git a/joint_state_controller/test/test_load_joint_state_controller.cpp b/joint_state_controller/test/test_load_joint_state_controller.cpp deleted file mode 100644 index bfba8a4743..0000000000 --- a/joint_state_controller/test/test_load_joint_state_controller.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointStateBroadcaster, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - // Even though joint_state_controller is deprecated and renamed to joint_state_broadcaster, it - // should still be loadable through its old name "joint_state_controller/JointStateController" - ASSERT_NO_THROW(cm.load_controller( - "test_joint_state_controller", "joint_state_controller/JointStateController")); - - rclcpp::shutdown(); -} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 576f29b80f..210e5073fb 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -17,7 +17,6 @@ forward_command_controller imu_sensor_broadcaster joint_state_broadcaster - joint_state_controller joint_trajectory_controller position_controllers velocity_controllers From bbcb68bd7116944dee2157cf2da5730540d0d1a4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 31 Aug 2021 21:32:57 +0100 Subject: [PATCH 015/524] Update CI to Galactic (#231) * Update CI to Galactic * Apply suggestions from code review * fixup --- .github/workflows/ci.yml | 12 ++++++------ .github/workflows/lint.yml | 4 ++-- .github/workflows/prerelease.yaml | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 66b4199243..b033b4a698 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,13 +10,13 @@ on: jobs: ci_binary: - name: Foxy binary job + name: Galactic binary job runs-on: ubuntu-latest strategy: matrix: env: - - {ROS_DISTRO: foxy, ROS_REPO: main} - - {ROS_DISTRO: foxy, ROS_REPO: testing} + - {ROS_DISTRO: galactic, ROS_REPO: main} + - {ROS_DISTRO: galactic, ROS_REPO: testing} env: UPSTREAM_WORKSPACE: .github/workspace.repos steps: @@ -25,17 +25,17 @@ jobs: env: ${{matrix.env}} ci_source: - name: Foxy source job + name: Galactic source job runs-on: ubuntu-20.04 strategy: fail-fast: false steps: - uses: ros-tooling/setup-ros@v0.2 with: - required-ros-distributions: foxy + required-ros-distributions: galactic - uses: ros-tooling/action-ros-ci@v0.2 with: - target-ros2-distro: foxy + target-ros2-distro: galactic # build all packages listed in the meta package package-name: | diff_drive_controller diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index d2f266d3fa..281ab5cd0a 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -15,7 +15,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: foxy + distribution: galactic linter: ${{ matrix.linter }} package-name: diff_drive_controller @@ -40,7 +40,7 @@ jobs: - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: foxy + distribution: galactic linter: cpplint arguments: "--filter=-whitespace/newline" package-name: diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml index 8b860855cf..b4a7b6f585 100644 --- a/.github/workflows/prerelease.yaml +++ b/.github/workflows/prerelease.yaml @@ -11,7 +11,7 @@ jobs: strategy: fail-fast: false matrix: - distro: [foxy, galactic, rolling] + distro: [galactic, rolling] env: ROS_DISTRO: ${{ matrix.distro }} From b4075e86948e655145c129d2d6d01eed2fbf2bc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 5 Sep 2021 13:29:48 +0200 Subject: [PATCH 016/524] Remove from ci.yaml (#234) * Remove from ci.yaml * Remove controller also from linters-CI --- .github/workflows/ci.yml | 1 - .github/workflows/lint.yml | 2 -- 2 files changed, 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b033b4a698..73c1418fb1 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -44,7 +44,6 @@ jobs: forward_command_controller imu_sensor_broadcaster joint_state_broadcaster - joint_state_controller joint_trajectory_controller gripper_controllers position_controllers diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 281ab5cd0a..5ac782b9b5 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -23,7 +23,6 @@ jobs: force_torque_sensor_broadcaster forward_command_controller joint_state_broadcaster - joint_state_controller joint_trajectory_controller gripper_controllers position_controllers @@ -49,7 +48,6 @@ jobs: force_torque_sensor_broadcaster forward_command_controller joint_state_broadcaster - joint_state_controller joint_trajectory_controller gripper_controllers position_controllers From eb1b11b003e2af0e01242eee780c0f1f9951cfe0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A1rk=20Szitanics?= Date: Sun, 5 Sep 2021 13:30:37 +0200 Subject: [PATCH 017/524] refactor get_current_state to get_state (#232) Based on this issue: https://github.com/ros-controls/ros2_control/issues/505 --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- .../src/joint_trajectory_controller.cpp | 4 ++-- .../test/test_trajectory_controller.cpp | 12 ++++++------ 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index f86b08c196..2bd6eb4dc4 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -142,7 +142,7 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons controller_interface::return_type DiffDriveController::update() { auto logger = node_->get_logger(); - if (get_current_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 37a93e0709..12bc4a2bc5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -107,7 +107,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update() { - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -900,7 +900,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( RCLCPP_INFO(node_->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 5f29d38ebb..d8896601c1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) executor.add_node(traj_controller_->get_node()->get_node_base_interface()); traj_controller_->configure(); - ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -268,14 +268,14 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); traj_controller_->configure(); - auto state = traj_controller_->get_current_state(); + auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - state = traj_controller_->get_current_state(); + state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -1107,10 +1107,10 @@ INSTANTIATE_TEST_CASE_P( TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { - EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->configure(); - EXPECT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); }; SetUpTrajectoryController(false); From 013511e0cf784cede4db51a7f83589d993595445 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 8 Sep 2021 12:00:12 +0530 Subject: [PATCH 018/524] Removing Boost from controllers. (#235) --- diff_drive_controller/CMakeLists.txt | 3 --- effort_controllers/CMakeLists.txt | 3 --- force_torque_sensor_broadcaster/CMakeLists.txt | 4 ---- forward_command_controller/CMakeLists.txt | 4 ---- gripper_controllers/CMakeLists.txt | 1 - imu_sensor_broadcaster/CMakeLists.txt | 4 ---- joint_state_broadcaster/CMakeLists.txt | 3 --- joint_trajectory_controller/CMakeLists.txt | 3 --- position_controllers/CMakeLists.txt | 3 --- velocity_controllers/CMakeLists.txt | 1 - 10 files changed, 29 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index d359d90968..2afd58bd60 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -45,8 +45,6 @@ ament_target_dependencies(diff_drive_controller # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(diff_drive_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) install(DIRECTORY include/ @@ -90,7 +88,6 @@ if(BUILD_TESTING) ament_target_dependencies(test_load_diff_drive_controller controller_manager ) - target_compile_definitions(test_load_diff_drive_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_accumulator diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index d290801917..dc13f1fd4e 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -29,8 +29,6 @@ ament_target_dependencies(effort_controllers # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(effort_controllers PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) install( @@ -60,7 +58,6 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - target_compile_definitions(test_load_joint_group_effort_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_joint_group_effort_controller diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 813a2ef2f9..d5b3f792f2 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -43,8 +43,6 @@ ament_target_dependencies( # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(force_torque_sensor_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) @@ -78,8 +76,6 @@ if(BUILD_TESTING) hardware_interface ros2_control_test_assets ) - # prevent pluginlib from using boost - target_compile_definitions(test_load_force_torque_sensor_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_force_torque_sensor_broadcaster diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 4f08cfd2f8..a600733a94 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -38,8 +38,6 @@ ament_target_dependencies(forward_command_controller # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(forward_command_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) install( @@ -71,8 +69,6 @@ if(BUILD_TESTING) hardware_interface ros2_control_test_assets ) - # prevent pluginlib from using boost - target_compile_definitions(test_load_forward_command_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_forward_command_controller diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 0414a9104c..07a472dcef 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -72,7 +72,6 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - target_compile_definitions(test_load_gripper_action_controllers PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") endif() ament_package() diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 8b8154aafd..2a530ab582 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -43,8 +43,6 @@ ament_target_dependencies( # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(imu_sensor_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) @@ -78,8 +76,6 @@ if(BUILD_TESTING) hardware_interface ros2_control_test_assets ) - # prevent pluginlib from using boost - target_compile_definitions(test_load_imu_sensor_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_imu_sensor_broadcaster diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index a656f4a334..ee29f9320b 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -35,8 +35,6 @@ ament_target_dependencies(joint_state_broadcaster # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(joint_state_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) install( @@ -69,7 +67,6 @@ if(BUILD_TESTING) hardware_interface ros2_control_test_assets ) - target_compile_definitions(test_load_joint_state_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_joint_state_broadcaster diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index a8a9a0a0a9..bcbb95d9a6 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -41,8 +41,6 @@ ament_target_dependencies(joint_trajectory_controller # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") -# prevent pluginlib from using boost -target_compile_definitions(joint_trajectory_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) install(DIRECTORY include/ @@ -92,7 +90,6 @@ if(BUILD_TESTING) realtime_tools ros2_control_test_assets ) - target_compile_definitions(test_load_joint_trajectory_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gtest( test_trajectory_actions diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 98beea35e4..be010fd1d7 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -29,8 +29,6 @@ ament_target_dependencies(position_controllers # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(position_controllers PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) install( @@ -60,7 +58,6 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - target_compile_definitions(test_load_joint_group_position_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_joint_group_position_controller diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 6107486f91..1635ace140 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -59,7 +59,6 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - target_compile_definitions(test_load_joint_group_velocity_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_add_gmock( test_joint_group_velocity_controller From bc440eef7634319fe6c001209d77083024489362 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 8 Sep 2021 21:22:39 +0530 Subject: [PATCH 019/524] ros2_controllers code changes to support ros2_controls issue #489 (#233) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial ros2_controller code changes to support ros2_control issue #489 * Repair tests where false method is used. Co-authored-by: Denis Štogl --- .../diff_drive_controller.hpp | 6 +-- .../src/diff_drive_controller.cpp | 13 ++----- .../joint_group_effort_controller.hpp | 2 +- .../src/joint_group_effort_controller.cpp | 11 +++--- .../test_joint_group_effort_controller.cpp | 2 +- .../force_torque_sensor_broadcaster.hpp | 5 +-- .../src/force_torque_sensor_broadcaster.cpp | 13 ++----- .../forward_command_controller.hpp | 6 +-- .../src/forward_command_controller.cpp | 13 ++----- .../gripper_action_controller.hpp | 6 +-- .../gripper_action_controller_impl.hpp | 31 ++++++++------- .../imu_sensor_broadcaster.hpp | 5 +-- .../src/imu_sensor_broadcaster.cpp | 12 ++---- .../joint_state_broadcaster.hpp | 6 +-- .../src/joint_state_broadcaster.cpp | 13 ++----- .../joint_trajectory_controller.hpp | 6 +-- .../src/joint_trajectory_controller.cpp | 39 ++++++++++--------- .../joint_group_position_controller.hpp | 3 +- .../src/joint_group_position_controller.cpp | 11 +++--- .../joint_group_velocity_controller.hpp | 3 +- .../src/joint_group_velocity_controller.cpp | 11 +++--- .../test_joint_group_velocity_controller.cpp | 2 +- 22 files changed, 91 insertions(+), 128 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index e9c07114dc..4b398c9c86 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 @@ -54,9 +54,6 @@ class DiffDriveController : public controller_interface::ControllerInterface DIFF_DRIVE_CONTROLLER_PUBLIC DiffDriveController(); - DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -66,6 +63,9 @@ class DiffDriveController : public controller_interface::ControllerInterface DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update() override; + DIFF_DRIVE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + DIFF_DRIVE_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2bd6eb4dc4..40d0421856 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -48,15 +48,8 @@ using lifecycle_msgs::msg::State; DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} -controller_interface::return_type DiffDriveController::init(const std::string & controller_name) +CallbackReturn DiffDriveController::on_init() { - // initialize lifecycle node - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - try { // with the lifecycle node being initialized, we can declare parameters @@ -105,10 +98,10 @@ controller_interface::return_type DiffDriveController::init(const std::string & catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } InterfaceConfiguration DiffDriveController::command_interface_configuration() const diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 903ab1dabd..5a9b9e3623 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -39,7 +39,7 @@ class JointGroupEffortController : public forward_command_controller::ForwardCom JointGroupEffortController(); EFFORT_CONTROLLERS_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; + CallbackReturn on_init() override; EFFORT_CONTROLLERS_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 82ea21892a..a5eea6d8a4 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -30,11 +30,10 @@ JointGroupEffortController::JointGroupEffortController() interface_name_ = hardware_interface::HW_IF_EFFORT; } -controller_interface::return_type JointGroupEffortController::init( - const std::string & controller_name) +CallbackReturn JointGroupEffortController::on_init() { - auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) + auto ret = ForwardCommandController::on_init(); + if (ret != CallbackReturn::SUCCESS) { return ret; } @@ -49,10 +48,10 @@ controller_interface::return_type JointGroupEffortController::init( catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn JointGroupEffortController::on_deactivate( diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index cc16573f5f..0e3ab94307 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("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/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 0439c97a7d..f1f4d1e8bf 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -41,15 +41,14 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC ForceTorqueSensorBroadcaster(); - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_init() override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 3a928f90e6..5f6fc7ec0b 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -28,15 +28,8 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() { } -controller_interface::return_type ForceTorqueSensorBroadcaster::init( - const std::string & controller_name) +CallbackReturn ForceTorqueSensorBroadcaster::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - try { auto_declare("sensor_name", ""); @@ -51,10 +44,10 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::init( catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn ForceTorqueSensorBroadcaster::on_configure( diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 1521fce0a2..bbbee94740 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -50,15 +50,15 @@ class ForwardCommandController : public controller_interface::ControllerInterfac FORWARD_COMMAND_CONTROLLER_PUBLIC ForwardCommandController(); - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + FORWARD_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + FORWARD_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 1c800d2ee7..7107cd03ce 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -35,15 +35,8 @@ ForwardCommandController::ForwardCommandController() { } -controller_interface::return_type ForwardCommandController::init( - const std::string & controller_name) +CallbackReturn ForwardCommandController::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - try { auto_declare>("joints", std::vector()); @@ -53,10 +46,10 @@ controller_interface::return_type ForwardCommandController::init( catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn ForwardCommandController::on_configure( diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index cbb9d216c9..ebf48741d0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -71,9 +71,6 @@ class GripperActionController : public controller_interface::ControllerInterface GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); - GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - /** * @brief command_interface_configuration This controller requires the * position command interfaces for the controlled joints @@ -91,6 +88,9 @@ class GripperActionController : public controller_interface::ControllerInterface GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update() override; + GRIPPER_ACTION_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + GRIPPER_ACTION_CONTROLLER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 4f3a994b95..c73d58e1f4 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -35,25 +35,26 @@ void GripperActionController::preempt_active_goal() } template -controller_interface::return_type GripperActionController::init( - const std::string & controller_name) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +GripperActionController::on_init() { - // initialize lifecycle node - const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) + try { - return ret; + // with the lifecycle node being initialized, we can declare parameters + auto_declare("action_monitor_rate", 20.0); + auto_declare("joint", joint_name_); + auto_declare("goal_tolerance", 0.01); + auto_declare("max_effort", 0.0); + auto_declare("stall_velocity_threshold", 0.001); + auto_declare("stall_timeout", 1.0); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - // with the lifecycle node being initialized, we can declare parameters - auto_declare("action_monitor_rate", 20.0); - auto_declare("joint", joint_name_); - auto_declare("goal_tolerance", 0.01); - auto_declare("max_effort", 0.0); - auto_declare("stall_velocity_threshold", 0.001); - auto_declare("stall_timeout", 1.0); - - return controller_interface::return_type::OK; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } template diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 39f4f9e11c..50607fcab5 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -38,15 +38,14 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class IMUSensorBroadcaster : public controller_interface::ControllerInterface { public: - IMU_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + IMU_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_init() override; + IMU_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 8ada847685..cb5e3d6f0b 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -23,14 +23,8 @@ namespace imu_sensor_broadcaster { -controller_interface::return_type IMUSensorBroadcaster::init(const std::string & controller_name) +CallbackReturn IMUSensorBroadcaster::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - try { auto_declare("sensor_name", ""); @@ -40,10 +34,10 @@ controller_interface::return_type IMUSensorBroadcaster::init(const std::string & { RCLCPP_ERROR( node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn IMUSensorBroadcaster::on_configure( diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 20f33722ec..0b163edb97 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -35,9 +35,6 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface JOINT_STATE_BROADCASTER_PUBLIC JointStateBroadcaster(); - JOINT_STATE_BROADCASTER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -47,6 +44,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface JOINT_STATE_BROADCASTER_PUBLIC controller_interface::return_type update() override; + JOINT_STATE_BROADCASTER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + JOINT_STATE_BROADCASTER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 59c9a73cb1..092a7a9531 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -46,14 +46,9 @@ using hardware_interface::HW_IF_VELOCITY; JointStateBroadcaster::JointStateBroadcaster() {} -controller_interface::return_type JointStateBroadcaster::init(const std::string & controller_name) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +JointStateBroadcaster::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; - } - try { auto_declare("use_local_topics", false); @@ -61,10 +56,10 @@ controller_interface::return_type JointStateBroadcaster::init(const std::string catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration 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 1092dc7287..fd720b0c42 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 @@ -64,9 +64,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC JointTrajectoryController(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; - /** * @brief command_interface_configuration This controller requires the position command * interfaces for the controlled joints @@ -84,6 +81,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::return_type update() override; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 12bc4a2bc5..0071c6e425 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -49,28 +49,29 @@ JointTrajectoryController::JointTrajectoryController() { } -controller_interface::return_type JointTrajectoryController::init( - const std::string & controller_name) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +JointTrajectoryController::on_init() { - // initialize lifecycle node - const auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) - { - return ret; + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare>("joints", joint_names_); + auto_declare>("command_interfaces", command_interface_types_); + auto_declare>("state_interfaces", state_interface_types_); + auto_declare("state_publish_rate", 50.0); + auto_declare("action_monitor_rate", 20.0); + auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); + auto_declare("open_loop_control", open_loop_control_); + auto_declare("constraints.stopped_velocity_tolerance", 0.01); + auto_declare("constraints.goal_time", 0.0); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - // with the lifecycle node being initialized, we can declare parameters - auto_declare>("joints", joint_names_); - auto_declare>("command_interfaces", command_interface_types_); - auto_declare>("state_interfaces", state_interface_types_); - auto_declare("state_publish_rate", 50.0); - auto_declare("action_monitor_rate", 20.0); - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - auto_declare("open_loop_control", open_loop_control_); - auto_declare("constraints.stopped_velocity_tolerance", 0.01); - auto_declare("constraints.goal_time", 0.0); - - return controller_interface::return_type::OK; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 2051ae44ab..dd1f4481d0 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -38,8 +38,7 @@ class JointGroupPositionController : public forward_command_controller::ForwardC POSITION_CONTROLLERS_PUBLIC JointGroupPositionController(); - POSITION_CONTROLLERS_PUBLIC controller_interface::return_type init( - const std::string & controller_name) override; + POSITION_CONTROLLERS_PUBLIC JointGroupPositionController::CallbackReturn on_init() override; }; } // namespace position_controllers diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index acbe4b62c3..411ec74749 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -30,11 +30,10 @@ JointGroupPositionController::JointGroupPositionController() interface_name_ = hardware_interface::HW_IF_POSITION; } -controller_interface::return_type JointGroupPositionController::init( - const std::string & controller_name) +CallbackReturn JointGroupPositionController::on_init() { - auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) + auto ret = ForwardCommandController::on_init(); + if (ret != CallbackReturn::SUCCESS) { return ret; } @@ -49,10 +48,10 @@ controller_interface::return_type JointGroupPositionController::init( catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } } // namespace position_controllers diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index 6e7ff3d26f..e5aeefa3a9 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -38,8 +38,7 @@ class JointGroupVelocityController : public forward_command_controller::ForwardC VELOCITY_CONTROLLERS_PUBLIC JointGroupVelocityController(); - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; + VELOCITY_CONTROLLERS_PUBLIC CallbackReturn on_init() override; VELOCITY_CONTROLLERS_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index b349b4ef45..ea846c162f 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -30,11 +30,10 @@ JointGroupVelocityController::JointGroupVelocityController() interface_name_ = hardware_interface::HW_IF_VELOCITY; } -controller_interface::return_type JointGroupVelocityController::init( - const std::string & controller_name) +CallbackReturn JointGroupVelocityController::on_init() { - auto ret = ForwardCommandController::init(controller_name); - if (ret != controller_interface::return_type::OK) + auto ret = ForwardCommandController::on_init(); + if (ret != CallbackReturn::SUCCESS) { return ret; } @@ -49,10 +48,10 @@ controller_interface::return_type JointGroupVelocityController::init( catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn JointGroupVelocityController::on_deactivate( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index c0dce9b4bf..e84ac69bdf 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("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 e281907f2a8d392e6c2f638963414f2851e99008 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 13 Sep 2021 04:28:14 -0600 Subject: [PATCH 020/524] Change test to work with Foxy and posterior action API (#237) Co-authored-by: Victor Lopez --- .../test/test_trajectory_actions.cpp | 57 +++++++------------ 1 file changed, 21 insertions(+), 36 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 89195279e0..65e756a4d2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -61,8 +61,6 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUp() { TrajectoryControllerTest::SetUp(); - goal_options_.goal_response_callback = - std::bind(&TestTrajectoryActions::common_goal_response, this, _1); goal_options_.result_callback = std::bind(&TestTrajectoryActions::common_result_response, this, _1); goal_options_.feedback_callback = nullptr; @@ -153,7 +151,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalHandle = rclcpp_action::ClientGoalHandle; using GoalOptions = rclcpp_action::Client::SendGoalOptions; - bool sendActionGoal( + std::shared_future sendActionGoal( const std::vector & points, double timeout, const GoalOptions & opt) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; @@ -161,13 +159,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; - auto goal_handle_future = action_client_->async_send_goal(goal_msg, opt); - return true; + return action_client_->async_send_goal(goal_msg, opt); } rclcpp_action::Client::SharedPtr action_client_; rclcpp_action::ResultCode common_resultcode_ = rclcpp_action::ResultCode::UNKNOWN; - bool common_goal_accepted_ = false; int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; bool setup_executor_ = false; @@ -180,23 +176,6 @@ class TestTrajectoryActions : public TrajectoryControllerTest GoalOptions goal_options_; public: - void common_goal_response(std::shared_future future) - { - RCLCPP_DEBUG( - node_->get_logger(), "common_goal_response time: %f", rclcpp::Clock().now().seconds()); - const auto goal_handle = future.get(); - if (!goal_handle) - { - common_goal_accepted_ = false; - RCLCPP_DEBUG(node_->get_logger(), "Goal rejected"); - } - else - { - common_goal_accepted_ = true; - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted"); - } - } - void common_result_response(const GoalHandle::WrappedResult & result) { RCLCPP_DEBUG( @@ -225,6 +204,7 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -238,11 +218,11 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ(1.0, joint_pos_[0]); @@ -262,6 +242,7 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -283,12 +264,12 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) point2.positions[2] = 9.0; points.push_back(point2); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); EXPECT_TRUE(feedback_recv); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); @@ -307,6 +288,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) SetUpExecutor(params); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -319,11 +301,11 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) point.positions[2] = 3.0; points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); @@ -351,6 +333,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -372,12 +355,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) point2.positions[2] = 9.0; points.push_back(point2); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); EXPECT_TRUE(feedback_recv); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); @@ -399,6 +382,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) SetUpExecutor(params); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -411,11 +395,11 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) point.positions[2] = 6.0; points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, @@ -427,6 +411,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -445,15 +430,15 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) goal_msg.trajectory.points = points; // send and wait for half a second before cancel - const auto goal_handle_future = action_client_->async_send_goal(goal_msg, goal_options_); + gh_future = action_client_->async_send_goal(goal_msg, goal_options_); std::this_thread::sleep_for(std::chrono::milliseconds(500)); - const auto goal_handle = goal_handle_future.get(); + const auto goal_handle = gh_future.get(); action_client_->async_cancel_goal(goal_handle); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); From 79ee39d944d2adaa30253ce1792696889c06b354 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 13 Sep 2021 12:35:57 +0200 Subject: [PATCH 021/524] Unify style of controllers. (#236) --- .../test/test_diff_drive_controller.cpp | 2 +- .../joint_group_effort_controller.hpp | 3 ++ .../src/joint_group_effort_controller.cpp | 5 +-- .../test_joint_group_effort_controller.cpp | 2 +- .../gripper_action_controller.hpp | 13 +++--- .../gripper_action_controller_impl.hpp | 43 +++++++++--------- .../joint_state_broadcaster.hpp | 13 +++--- .../src/joint_state_broadcaster.cpp | 29 ++++++------ .../joint_trajectory_controller.hpp | 22 ++++----- .../src/joint_trajectory_controller.cpp | 45 ++++++++----------- .../joint_group_position_controller.hpp | 5 ++- position_controllers/package.xml | 1 - .../src/joint_group_position_controller.cpp | 5 +-- .../test_joint_group_position_controller.cpp | 2 +- .../joint_group_velocity_controller.hpp | 2 + .../src/joint_group_velocity_controller.cpp | 3 +- .../test_joint_group_velocity_controller.cpp | 2 +- 17 files changed, 95 insertions(+), 102 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index a3dda0b087..40320248d4 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -28,7 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" -using CallbackReturn = diff_drive_controller::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::LoanedCommandInterface; diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 5a9b9e3623..a7228a2cae 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -19,9 +19,12 @@ #include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace effort_controllers { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + /** * \brief Forward command controller for a set of effort controlled joints (linear or angular). * diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index a5eea6d8a4..e607e3c27f 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -14,6 +14,7 @@ #include +#include "controller_interface/controller_interface.hpp" #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" @@ -21,8 +22,6 @@ namespace effort_controllers { -using CallbackReturn = JointGroupEffortController::CallbackReturn; - JointGroupEffortController::JointGroupEffortController() : forward_command_controller::ForwardCommandController() { @@ -32,7 +31,7 @@ JointGroupEffortController::JointGroupEffortController() CallbackReturn JointGroupEffortController::on_init() { - auto ret = ForwardCommandController::on_init(); + auto ret = forward_command_controller::ForwardCommandController::on_init(); if (ret != CallbackReturn::SUCCESS) { return ret; diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 0e3ab94307..3864aef639 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" -using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index ebf48741d0..a980ca5388 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -47,6 +47,8 @@ namespace gripper_action_controller { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + /** * \brief Controller for executing a gripper command action for simple * single-dof grippers. @@ -89,19 +91,16 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::return_type update() override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + CallbackReturn on_init() override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; // pre-allocated memory that is re-used to set the realtime buffer diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index c73d58e1f4..cff250d1f6 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -17,11 +17,15 @@ #ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ #define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#include "gripper_controllers/gripper_action_controller.hpp" + #include #include namespace gripper_action_controller { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + template void GripperActionController::preempt_active_goal() { @@ -35,8 +39,7 @@ void GripperActionController::preempt_active_goal() } template -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_init() +CallbackReturn GripperActionController::on_init() { try { @@ -51,10 +54,10 @@ GripperActionController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } template @@ -184,8 +187,8 @@ void GripperActionController::check_for_success( } template -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_configure(const rclcpp_lifecycle::State &) +CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) { const auto logger = node_->get_logger(); @@ -201,7 +204,7 @@ GripperActionController::on_configure(const rclcpp_lifecycle: if (joint_name_.empty()) { RCLCPP_ERROR(logger, "Could not find joint name on param server"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } // Default tolerances @@ -214,11 +217,11 @@ GripperActionController::on_configure(const rclcpp_lifecycle: stall_velocity_threshold_ = node_->get_parameter("stall_velocity_threshold").as_double(); stall_timeout_ = node_->get_parameter("stall_timeout").as_double(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } template -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) { auto position_command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), @@ -228,7 +231,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: if (position_command_interface_it == command_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position command interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } if (position_command_interface_it->get_name() != joint_name_) { @@ -236,7 +239,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: node_->get_logger(), "Position command interface is different than joint name `" << position_command_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), @@ -246,7 +249,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: if (position_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position state interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } if (position_state_interface_it->get_name() != joint_name_) { @@ -254,7 +257,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: node_->get_logger(), "Position state interface is different than joint name `" << position_state_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), @@ -264,7 +267,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: if (velocity_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 velocity state interface"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } if (velocity_state_interface_it->get_name() != joint_name_) { @@ -272,7 +275,7 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: node_->get_logger(), "Velocity command interface is different than joint name `" << velocity_state_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } joint_position_command_interface_ = *position_command_interface_it; @@ -300,18 +303,18 @@ GripperActionController::on_activate(const rclcpp_lifecycle:: std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } template -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -GripperActionController::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) { joint_position_command_interface_ = std::experimental::nullopt; joint_position_state_interface_ = std::experimental::nullopt; joint_velocity_state_interface_ = std::experimental::nullopt; release_interfaces(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } template diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 0b163edb97..ff55d0210e 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -29,6 +29,8 @@ namespace joint_state_broadcaster { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + class JointStateBroadcaster : public controller_interface::ControllerInterface { public: @@ -45,19 +47,16 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface controller_interface::return_type update() override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + CallbackReturn on_init() override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; protected: bool init_joint_data(); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 092a7a9531..9bc089085f 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -46,8 +46,7 @@ using hardware_interface::HW_IF_VELOCITY; JointStateBroadcaster::JointStateBroadcaster() {} -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointStateBroadcaster::on_init() +CallbackReturn JointStateBroadcaster::on_init() { try { @@ -56,10 +55,10 @@ JointStateBroadcaster::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration @@ -76,8 +75,8 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf controller_interface::interface_configuration_type::ALL}; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn JointStateBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); @@ -96,29 +95,29 @@ JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_s { // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointStateBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn JointStateBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { if (!init_joint_data()) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } init_joint_state_msg(); init_dynamic_joint_state_msg(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn JointStateBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } template 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 fd720b0c42..9e66c35073 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 @@ -56,6 +56,8 @@ class State; namespace joint_trajectory_controller { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface @@ -82,31 +84,25 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::return_type update() override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + CallbackReturn on_init() override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; protected: std::vector joint_names_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0071c6e425..6107c4406a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -49,8 +49,7 @@ JointTrajectoryController::JointTrajectoryController() { } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_init() +CallbackReturn JointTrajectoryController::on_init() { try { @@ -68,10 +67,10 @@ JointTrajectoryController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration @@ -414,8 +413,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) { const auto logger = node_->get_logger(); @@ -424,7 +422,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) if (!reset()) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return CallbackReturn::FAILURE; } if (joint_names_.empty()) @@ -695,7 +693,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } // Fill ordered_interfaces with references to the matching interfaces @@ -720,8 +718,7 @@ bool get_ordered_interfaces( return joint_names.size() == ordered_interfaces.size(); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { // order all joints in the storage for (const auto & interface : command_interface_types_) @@ -735,7 +732,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_ERROR( node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } } for (const auto & interface : state_interface_types_) @@ -749,7 +746,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_ERROR( node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } } @@ -789,11 +786,10 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) } // TODO(karsten1987): activate subscriptions of subscriber - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) { // TODO(anyone): How to halt when using effort commands? for (auto index = 0ul; index < joint_names_.size(); ++index) @@ -811,27 +807,25 @@ JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) subscriber_is_active_ = false; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::State &) { // go home traj_home_point_ptr_->update(traj_msg_home_ptr_); traj_point_active_ptr_ = &traj_home_point_ptr_; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_error(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_error(const rclcpp_lifecycle::State &) { if (!reset()) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } bool JointTrajectoryController::reset() @@ -849,12 +843,11 @@ bool JointTrajectoryController::reset() return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -JointTrajectoryController::on_shutdown(const rclcpp_lifecycle::State &) +CallbackReturn JointTrajectoryController::on_shutdown(const rclcpp_lifecycle::State &) { // TODO(karsten1987): what to do? - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } void JointTrajectoryController::publish_state( diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index dd1f4481d0..0b3cb0796b 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -19,9 +19,12 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "position_controllers/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace position_controllers { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + /** * \brief Forward command controller for a set of position controlled joints (linear or angular). * @@ -38,7 +41,7 @@ class JointGroupPositionController : public forward_command_controller::ForwardC POSITION_CONTROLLERS_PUBLIC JointGroupPositionController(); - POSITION_CONTROLLERS_PUBLIC JointGroupPositionController::CallbackReturn on_init() override; + POSITION_CONTROLLERS_PUBLIC CallbackReturn on_init() override; }; } // namespace position_controllers diff --git a/position_controllers/package.xml b/position_controllers/package.xml index f990c94e57..97dd8ebad6 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -17,7 +17,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 411ec74749..e52f9425fd 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -14,6 +14,7 @@ #include +#include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" #include "rclcpp/logging.hpp" @@ -21,8 +22,6 @@ namespace position_controllers { -using CallbackReturn = JointGroupPositionController::CallbackReturn; - JointGroupPositionController::JointGroupPositionController() : forward_command_controller::ForwardCommandController() { @@ -32,7 +31,7 @@ JointGroupPositionController::JointGroupPositionController() CallbackReturn JointGroupPositionController::on_init() { - auto ret = ForwardCommandController::on_init(); + auto ret = forward_command_controller::ForwardCommandController::on_init(); if (ret != CallbackReturn::SUCCESS) { return ret; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index f07561d4fb..2c8ddecb7f 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" -using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index e5aeefa3a9..2c45448cf2 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -18,10 +18,12 @@ #include #include "forward_command_controller/forward_command_controller.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "velocity_controllers/visibility_control.h" namespace velocity_controllers { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** * \brief Forward command controller for a set of velocity controlled joints (linear or angular). * diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index ea846c162f..c2c7d762bd 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -14,6 +14,7 @@ #include +#include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" @@ -21,8 +22,6 @@ namespace velocity_controllers { -using CallbackReturn = JointGroupVelocityController::CallbackReturn; - JointGroupVelocityController::JointGroupVelocityController() : forward_command_controller::ForwardCommandController() { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index e84ac69bdf..d2c6cbc57a 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" -using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace From 19b1c326a86cee71dca9eb8d156045c2e14d3172 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 17 Sep 2021 23:36:46 +0200 Subject: [PATCH 022/524] =?UTF-8?q?Quickfix=20=F0=9F=9B=A0:=20Correct=20co?= =?UTF-8?q?nfusing=20variable=20name=20(#240)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/joint_trajectory_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6107c4406a..3e3f11f605 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -586,16 +586,16 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } auto get_interface_list = [](const std::vector & interface_types) { - std::stringstream ss_command_interfaces; + std::stringstream ss_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { if (index != 0) { - ss_command_interfaces << " "; + ss_interfaces << " "; } - ss_command_interfaces << interface_types[index]; + ss_interfaces << interface_types[index]; } - return ss_command_interfaces.str(); + return ss_interfaces.str(); }; // Print output so users can be sure the interface setup is correct From 177b174ac0fdb91bb4e68815f6cf349425212988 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Sep 2021 09:48:21 +0100 Subject: [PATCH 023/524] Add time and period to update function (#241) * Add time and period to update function Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com> --- .../diff_drive_controller.hpp | 3 +- .../src/diff_drive_controller.cpp | 25 +++++----- .../test/test_diff_drive_controller.cpp | 16 +++++-- .../test_joint_group_effort_controller.cpp | 20 ++++++-- .../force_torque_sensor_broadcaster.hpp | 3 +- .../src/force_torque_sensor_broadcaster.cpp | 5 +- .../test_force_torque_sensor_broadcaster.cpp | 12 +++-- .../forward_command_controller.hpp | 3 +- .../src/forward_command_controller.cpp | 3 +- .../test/test_forward_command_controller.cpp | 20 ++++++-- .../gripper_action_controller.hpp | 3 +- .../gripper_action_controller_impl.hpp | 3 +- .../imu_sensor_broadcaster.hpp | 3 +- .../src/imu_sensor_broadcaster.cpp | 5 +- .../test/test_imu_sensor_broadcaster.cpp | 8 +++- .../joint_state_broadcaster.hpp | 3 +- .../src/joint_state_broadcaster.cpp | 7 +-- .../test/test_joint_state_broadcaster.cpp | 12 +++-- .../joint_trajectory_controller.hpp | 3 +- .../src/joint_trajectory_controller.cpp | 3 +- .../test/test_trajectory_actions.cpp | 4 +- .../test/test_trajectory_controller.cpp | 48 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 2 +- .../test_joint_group_position_controller.cpp | 20 ++++++-- .../test_joint_group_velocity_controller.cpp | 20 ++++++-- 25 files changed, 166 insertions(+), 88 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 4b398c9c86..38f767dd3b 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 @@ -61,7 +61,8 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; DIFF_DRIVE_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 40d0421856..e816b26f11 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -132,7 +132,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons return {interface_configuration_type::INDIVIDUAL, conf_names}; } -controller_interface::return_type DiffDriveController::update() +controller_interface::return_type DiffDriveController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto logger = node_->get_logger(); if (get_state().id() == State::PRIMARY_STATE_INACTIVE) @@ -145,29 +146,29 @@ controller_interface::return_type DiffDriveController::update() return controller_interface::return_type::OK; } - const auto current_time = node_->get_clock()->now(); + const auto current_time = time; - std::shared_ptr last_msg; - received_velocity_msg_ptr_.get(last_msg); + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); - if (last_msg == nullptr) + if (last_command_msg == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto dt = current_time - last_msg->header.stamp; + const auto age_of_last_command = current_time - last_command_msg->header.stamp; // Brake if cmd_vel has timeout, override the stored command - if (dt > cmd_vel_timeout_) + if (age_of_last_command > cmd_vel_timeout_) { - last_msg->twist.linear.x = 0.0; - last_msg->twist.angular.z = 0.0; + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; } // linear_command and angular_command may be limited further by SpeedLimit, // without affecting the stored twist command - double linear_command = last_msg->twist.linear.x; - double angular_command = last_msg->twist.angular.z; + double linear_command = last_command_msg->twist.linear.x; + double angular_command = last_command_msg->twist.angular.z; // Apply (possibly new) multipliers: const auto wheels = wheel_params_; @@ -247,7 +248,7 @@ controller_interface::return_type DiffDriveController::update() angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); previous_commands_.pop(); - previous_commands_.emplace(*last_msg); + previous_commands_.emplace(*last_command_msg); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 40320248d4..ca3da08803 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -285,11 +285,15 @@ TEST_F(TestDiffDriveController, cleanup) publish(linear, angular); controller_->wait_for_twist(executor); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); state = controller_->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); state = controller_->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -333,7 +337,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) // wait for msg is be published to the system ASSERT_TRUE(controller_->wait_for_twist(executor)); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); @@ -342,7 +348,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) std::this_thread::sleep_for(std::chrono::milliseconds(500)); state = controller_->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 3864aef639..157384423a 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index f1f4d1e8bf..a1c0be6335 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -59,7 +59,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::string sensor_name_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 5f6fc7ec0b..5f5c880c39 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -156,11 +156,12 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update() +controller_interface::return_type ForceTorqueSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); + realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); realtime_publisher_->unlockAndPublish(); } 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 3d61eb5490..db8f6e7022 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 @@ -88,7 +88,9 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -220,7 +222,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) @@ -234,7 +238,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index bbbee94740..9926a7e9b7 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -69,7 +69,8 @@ class ForwardCommandController : public controller_interface::ControllerInterfac CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::vector joint_names_; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 7107cd03ce..49284ae5cb 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -153,7 +153,8 @@ CallbackReturn ForwardCommandController::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type ForwardCommandController::update() +controller_interface::return_type ForwardCommandController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index e42fe6e61a..425c08c7af 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -178,7 +178,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -191,7 +193,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); @@ -215,7 +219,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -233,7 +239,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -274,7 +282,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index a980ca5388..c041fb2de1 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -88,7 +88,8 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; GRIPPER_ACTION_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index cff250d1f6..211e3cd4d5 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -61,7 +61,8 @@ CallbackReturn GripperActionController::on_init() } template -controller_interface::return_type GripperActionController::update() +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { command_struct_rt_ = *(command_.readFromRT()); diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 50607fcab5..9a736d5cfb 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -56,7 +56,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; IMU_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::string sensor_name_; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index cb5e3d6f0b..23aa33777c 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -112,11 +112,12 @@ CallbackReturn IMUSensorBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type IMUSensorBroadcaster::update() +controller_interface::return_type IMUSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); + realtime_publisher_->msg_.header.stamp = time; imu_sensor_->get_values_as_message(realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 3edc555dd7..d27be76b3b 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -90,7 +90,9 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & "/test_imu_sensor_broadcaster/imu", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -171,7 +173,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index ff55d0210e..175c3610f6 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -44,7 +44,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_STATE_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_STATE_BROADCASTER_PUBLIC CallbackReturn on_init() override; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 9bc089085f..c5dbcc4c82 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -229,7 +229,8 @@ double get_value( } } -controller_interface::return_type JointStateBroadcaster::update() +controller_interface::return_type JointStateBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { for (const auto & state_interface : state_interfaces_) { @@ -240,8 +241,8 @@ controller_interface::return_type JointStateBroadcaster::update() state_interface.get_interface_name().c_str(), state_interface.get_value()); } - joint_state_msg_.header.stamp = get_node()->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now(); + joint_state_msg_.header.stamp = time; + dynamic_joint_state_msg_.header.stamp = time; // update joint state message and dynamic joint state message for (auto i = 0ul; i < joint_names_.size(); ++i) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 5bf3d9dc3d..f38c108589 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -184,7 +184,9 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) auto node_state = state_broadcaster_->configure(); node_state = state_broadcaster_->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -200,7 +202,9 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -250,7 +254,9 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); 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 9e66c35073..786ef42b0b 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 @@ -81,7 +81,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3e3f11f605..843ce78459 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -105,7 +105,8 @@ JointTrajectoryController::state_interface_configuration() const return conf; } -controller_interface::return_type JointTrajectoryController::update() +controller_interface::return_type JointTrajectoryController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 65e756a4d2..5bb6efaea6 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -94,7 +94,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest auto end_time = start_time + wait; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); } }); @@ -448,7 +448,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) const double prev_pos3 = joint_pos_[2]; // run an update, it should be holding - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_EQ(prev_pos1, joint_pos_[0]); EXPECT_EQ(prev_pos2, joint_pos_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d8896601c1..0a992b14f7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -79,7 +79,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) publish(time_from_start, points); std::this_thread::sleep_for(std::chrono::milliseconds(10)); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // hw position == 0 because controller is not activated EXPECT_EQ(0.0, joint_pos_[0]); @@ -144,7 +144,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // executor.spin_once(); // -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // change in hw position @@ -192,7 +192,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // std::this_thread::sleep_for(std::chrono::milliseconds(500)); // executor.spin_once(); // -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // deactivated @@ -201,7 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // state = traj_controller_->deactivate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // resource_manager_->read(); -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // no change in hw position @@ -215,7 +215,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // state = traj_node->activate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // resource_manager_->read(); -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // change in hw position to 3rd point @@ -241,11 +241,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) std::vector> points{{{3.3, 4.4, 5.5}}}; publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); auto state = traj_controller_->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); state = traj_controller_->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -292,11 +292,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->wait_for_trajectory(executor); // first update - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // wait so controller process the second point when deactivated std::this_thread::sleep_for(FIRST_POINT_TIME); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // deactivated state = traj_controller_->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -313,11 +313,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param state = traj_controller_->cleanup(); // update loop receives a new msg and updates accordingly - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check the traj_msg_home_ptr_ initialization code for the standard wait timing std::this_thread::sleep_for(std::chrono::milliseconds(50)); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], allowed_delta); @@ -404,7 +404,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou const auto end_time = start_time + wait; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); } // We may miss the last message since time allowed is exactly the time needed @@ -832,15 +832,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -859,15 +859,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -916,15 +916,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // There should not be backward commands EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -943,15 +943,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // There should not be a jump toward commands EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5224a64aeb..8ece5c393a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -303,7 +303,7 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); } } diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 2c8ddecb7f..be774031f0 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index d2c6cbc57a..b566829f9b 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); From 554995e311f896c02bd7c90b2c4cb6f7c01a67d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 21 Sep 2021 15:01:26 +0200 Subject: [PATCH 024/524] Remove compile warnings. (#245) * Remove compile warnings. * correct formatting. --- .../src/forward_command_controller.cpp | 2 +- .../gripper_controllers/gripper_action_controller_impl.hpp | 5 +++-- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_controller.cpp | 6 +++--- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 49284ae5cb..962586f083 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -154,7 +154,7 @@ CallbackReturn ForwardCommandController::on_deactivate( } controller_interface::return_type ForwardCommandController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 211e3cd4d5..9598d03bb1 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -62,7 +62,7 @@ CallbackReturn GripperActionController::on_init() template controller_interface::return_type GripperActionController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { command_struct_rt_ = *(command_.readFromRT()); @@ -339,7 +339,8 @@ GripperActionController::state_interface_configuration() cons template GripperActionController::GripperActionController() -: controller_interface::ControllerInterface(), action_monitor_period_(0) +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) { } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 843ce78459..33de8253b9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -106,7 +106,7 @@ JointTrajectoryController::state_interface_configuration() const } controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0a992b14f7..130f602c59 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1067,7 +1067,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co // TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P // position controllers -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple(std::vector({"position"}), std::vector({"position"})), @@ -1078,7 +1078,7 @@ INSTANTIATE_TEST_CASE_P( std::vector({"position", "velocity", "acceleration"})))); // position_velocity controllers -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( @@ -1091,7 +1091,7 @@ INSTANTIATE_TEST_CASE_P( std::vector({"position", "velocity", "acceleration"})))); // position_velocity_acceleration controllers -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( From 4a9b6cd6fe626e885dd2c64beb260a9ecb020a77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 24 Sep 2021 19:49:10 +0200 Subject: [PATCH 025/524] Reset and test of command buffer for forwarding controllers. (#246) --- .../src/forward_command_controller.cpp | 6 ++ .../test/test_forward_command_controller.cpp | 96 ++++++++++++++++++- .../test/test_forward_command_controller.hpp | 1 + 3 files changed, 100 insertions(+), 3 deletions(-) diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 962586f083..2a4f2d2444 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -15,6 +15,7 @@ #include "forward_command_controller/forward_command_controller.hpp" #include +#include #include #include #include @@ -144,12 +145,17 @@ CallbackReturn ForwardCommandController::on_activate( return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + return CallbackReturn::SUCCESS; } CallbackReturn ForwardCommandController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); return CallbackReturn::SUCCESS; } diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 425c08c7af..47ed8b7f87 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -22,6 +20,8 @@ #include "gmock/gmock.h" +#include "test_forward_command_controller.hpp" + #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -32,7 +32,6 @@ #include "rclcpp/utilities.hpp" #include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "test_forward_command_controller.hpp" using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; using hardware_interface::LoanedCommandInterface; @@ -291,3 +290,94 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); } + +TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); + + // default values + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto command_msg = std::make_shared(); + command_msg->data = {10.0, 20.0, 30.0}; + + controller_->rt_command_ptr_.writeFromNonRT(command_msg); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + + node_state = controller_->deactivate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // command ptr should be reset (nullptr) after deactivation - same check as in `update` + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // Controller is inactive but let's put some data into buffer (simulate callback when inactive) + command_msg = std::make_shared(); + command_msg->data = {5.5, 6.6, 7.7}; + + controller_->rt_command_ptr_.writeFromNonRT(command_msg); + + // command ptr should be available and message should be there - same check as in `update` + ASSERT_TRUE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_TRUE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // Now activate again + node_state = controller_->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // command ptr should be reset (nullptr) after activation - same check as in `update` + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // values should not change + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + + // set commands again + controller_->rt_command_ptr_.writeFromNonRT(command_msg); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); +} diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 5a5ef32f48..9c6bd2a352 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -44,6 +44,7 @@ class FriendForwardCommandController : public forward_command_controller::Forwar FRIEND_TEST(ForwardCommandControllerTest, WrongCommandCheckTest); FRIEND_TEST(ForwardCommandControllerTest, NoCommandCheckTest); FRIEND_TEST(ForwardCommandControllerTest, CommandCallbackTest); + FRIEND_TEST(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess); }; class ForwardCommandControllerTest : public ::testing::Test From 69d3460f39b2e4c8c95effc2df296693b8c41597 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Sep 2021 11:15:38 +0100 Subject: [PATCH 026/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 9 +++++++++ effort_controllers/CHANGELOG.rst | 8 ++++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ forward_command_controller/CHANGELOG.rst | 9 +++++++++ gripper_controllers/CHANGELOG.rst | 9 +++++++++ imu_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ joint_state_broadcaster/CHANGELOG.rst | 8 ++++++++ joint_trajectory_controller/CHANGELOG.rst | 12 ++++++++++++ position_controllers/CHANGELOG.rst | 8 ++++++++ ros2_controllers/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 8 ++++++++ 11 files changed, 90 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index d6690cab9d..3a4e858b02 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* refactor get_current_state to get_state (`#232 `_) +* Contributors: Bence Magyar, Denis Štogl, Márk Szitanics, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 35429e42a2..ba6638df19 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8ab8021ba1..8703e077ae 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 098499a771..4c6a14b855 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Reset and test of command buffer for forwarding controllers. (`#246 `_) +* Remove compile warnings. (`#245 `_) +* Add time and period to update function (`#241 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a827b6898a..103c5211f3 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove compile warnings. (`#245 `_) +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ba443fc8fa..b5df60cad1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 65ab8f1f69..2e7220ee88 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ddf376b1bc..fdf5c86cff 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove compile warnings. (`#245 `_) +* Add time and period to update function (`#241 `_) +* Quickfix 🛠: Correct confusing variable name (`#240 `_) +* Unify style of controllers. (`#236 `_) +* Change test to work with Foxy and posterior action API (`#237 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* refactor get_current_state to get_state (`#232 `_) +* Contributors: Bence Magyar, Denis Štogl, Márk Szitanics, Tyler Weaver, bailaC + 0.5.0 (2021-08-30) ------------------ * Add auto declaration of parameters. (`#224 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 31a74e0ca3..126b3bb5b6 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index d3d9681757..9cd118ff1f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove joint_state_controller, use joint_state_broadcaster instead (`#230 `_) +* Contributors: Bence Magyar + 0.5.0 (2021-08-30) ------------------ * Add initial pre-commit setup. (`#220 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 7e3aee7ac1..afade33513 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* Contributors: Bence Magyar, Denis Štogl, bailaC + 0.5.0 (2021-08-30) ------------------ * Bring precommit config up to speed with ros2_control (`#227 `_) From 8e40fd5791805e3d1435992b5134673aecddc25b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Sep 2021 11:22:09 +0100 Subject: [PATCH 027/524] 1.0.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3a4e858b02..5d8a0ea8dc 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9e1207679d..7132e90f98 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 0.5.0 + 1.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ba6638df19..6e933ac332 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 786c55681b..dc01f53385 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 0.5.0 + 1.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8703e077ae..0cc98d97dd 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 ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) * Removing Boost from controllers. (`#235 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 0daf5d5f40..1017afb3e5 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 0.5.0 + 1.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4c6a14b855..7d17f602a1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Reset and test of command buffer for forwarding controllers. (`#246 `_) * Remove compile warnings. (`#245 `_) * Add time and period to update function (`#241 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8d2aa4bf22..63308f672c 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 0.5.0 + 1.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 103c5211f3..c9acf9b2b9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Remove compile warnings. (`#245 `_) * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index accb5ab0c5..a8e2808687 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 0.5.0 + 1.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b5df60cad1..02abea3aec 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) * Removing Boost from controllers. (`#235 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 83b38f0b56..d5400f7820 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 0.5.0 + 1.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 2e7220ee88..e6d452396d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 155e4526ed..ec493cb6b6 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 0.5.0 + 1.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index fdf5c86cff..886dc150b3 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Remove compile warnings. (`#245 `_) * Add time and period to update function (`#241 `_) * Quickfix 🛠: Correct confusing variable name (`#240 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index e55ce46020..5a316e491f 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 0.5.0 + 1.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 126b3bb5b6..12e7957197 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 97dd8ebad6..c9362a9491 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 0.5.0 + 1.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 9cd118ff1f..b51c5c809e 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Remove joint_state_controller, use joint_state_broadcaster instead (`#230 `_) * Contributors: Bence Magyar diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 210e5073fb..181034d9de 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 0.5.0 + 1.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index afade33513..11a368304b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.0.0 (2021-09-29) +------------------ * Add time and period to update function (`#241 `_) * Unify style of controllers. (`#236 `_) * ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8656057ddd..749da4f841 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 0.5.0 + 1.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From d99826ac72b69c740055ecfbbfe058ca09f6b3c6 Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 15 Oct 2021 13:09:54 +0530 Subject: [PATCH 028/524] Adding changes for Issue-58. (#254) --- .../src/forward_command_controller.cpp | 2 +- .../src/joint_state_broadcaster.cpp | 6 ++--- .../test/test_joint_state_broadcaster.cpp | 2 +- .../tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 22 +++++++++---------- .../src/trajectory.cpp | 2 +- .../test/test_trajectory_controller.cpp | 8 +++---- 7 files changed, 22 insertions(+), 22 deletions(-) diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 2a4f2d2444..bc725e509f 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -179,7 +179,7 @@ controller_interface::return_type ForwardCommandController::update( return controller_interface::return_type::ERROR; } - for (auto index = 0ul; index < command_interfaces_.size(); ++index) + for (size_t index = 0; index < command_interfaces_.size(); ++index) { command_interfaces_[index].set_value((*joint_commands)->data[index]); } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c5dbcc4c82..178a417d54 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -245,7 +245,7 @@ controller_interface::return_type JointStateBroadcaster::update( dynamic_joint_state_msg_.header.stamp = time; // update joint state message and dynamic joint state message - for (auto i = 0ul; i < joint_names_.size(); ++i) + for (size_t i = 0; i < joint_names_.size(); ++i) { joint_state_msg_.position[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION); @@ -254,11 +254,11 @@ controller_interface::return_type JointStateBroadcaster::update( joint_state_msg_.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT); } - for (auto joint_index = 0ul; joint_index < dynamic_joint_state_msg_.joint_names.size(); + for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg_.joint_names.size(); ++joint_index) { const auto & name = dynamic_joint_state_msg_.joint_names[joint_index]; - for (auto interface_index = 0ul; + for (size_t interface_index = 0; interface_index < dynamic_joint_state_msg_.interface_values[joint_index].interface_names.size(); ++interface_index) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index f38c108589..8dc92993a0 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -273,7 +273,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (auto i = 0ul; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) { ASSERT_THAT( dynamic_joint_state_msg.interface_values[i].interface_names, diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 2d247eeb8b..b2944f7622 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -105,7 +105,7 @@ SegmentTolerances get_segment_tolerances( tolerances.state_tolerance.resize(n_joints); tolerances.goal_state_tolerance.resize(n_joints); - for (auto i = 0ul; i < n_joints; ++i) + for (size_t i = 0; i < n_joints; ++i) { const std::string prefix = "constraints." + joint_names[i]; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 33de8253b9..0dd678c354 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -148,7 +148,7 @@ controller_interface::return_type JointTrajectoryController::update( // changed, but its value only? auto assign_interface_from_point = [&, joint_num](auto & joint_inteface, const std::vector & trajectory_point_interface) { - for (auto index = 0ul; index < joint_num; ++index) + for (size_t index = 0; index < joint_num; ++index) { joint_inteface[index].get().set_value(trajectory_point_interface[index]); } @@ -208,7 +208,7 @@ controller_interface::return_type JointTrajectoryController::update( // assign_interface_from_point(joint_command_interface_[3], state_desired.effort); // } - for (auto index = 0ul; index < joint_num; ++index) + for (size_t index = 0; index < joint_num; ++index) { compute_error_for_joint(state_error, index, state_current, state_desired); @@ -314,7 +314,7 @@ void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & const auto joint_num = joint_names_.size(); auto assign_point_from_interface = [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { - for (auto index = 0ul; index < joint_num; ++index) + for (size_t index = 0; index < joint_num; ++index) { trajectory_point_interface[index] = joint_inteface[index].get().get_value(); } @@ -353,7 +353,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto const auto joint_num = joint_names_.size(); auto assign_point_from_interface = [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { - for (auto index = 0ul; index < joint_num; ++index) + for (size_t index = 0; index < joint_num; ++index) { trajectory_point_interface[index] = joint_inteface[index].get().get_value(); } @@ -793,13 +793,13 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) { // TODO(anyone): How to halt when using effort commands? - for (auto index = 0ul; index < joint_names_.size(); ++index) + for (size_t index = 0; index < joint_names_.size(); ++index) { joint_command_interface_[0][index].get().set_value( joint_command_interface_[0][index].get().get_value()); } - for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) { joint_command_interface_[index].clear(); joint_state_interface_[index].clear(); @@ -970,7 +970,7 @@ void JointTrajectoryController::fill_partial_goal( trajectory_msg->joint_names.reserve(joint_names_.size()); - for (auto index = 0ul; index < joint_names_.size(); ++index) + for (size_t index = 0; index < joint_names_.size(); ++index) { { if ( @@ -1023,7 +1023,7 @@ void JointTrajectoryController::sort_to_local_joint_order( } std::vector output; output.resize(mapping.size(), 0.0); - for (auto index = 0ul; index < mapping.size(); ++index) + for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index]; output[map_index] = to_remap[index]; @@ -1031,7 +1031,7 @@ void JointTrajectoryController::sort_to_local_joint_order( return output; }; - for (auto index = 0ul; index < trajectory_msg->points.size(); ++index) + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) { trajectory_msg->points[index].positions = remap(trajectory_msg->points[index].positions, mapping_vector); @@ -1106,7 +1106,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) + for (size_t i = 0; i < trajectory.joint_names.size(); ++i) { const std::string & incoming_joint_name = trajectory.joint_names[i]; @@ -1121,7 +1121,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } rclcpp::Duration previous_traj_time(0ms); - for (auto i = 0ul; i < trajectory.points.size(); ++i) + for (size_t i = 0; i < trajectory.points.size(); ++i) { if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 83ba25d2d6..0ad77ff564 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -106,7 +106,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (auto i = 0ul; i < last_idx; ++i) + for (size_t i = 0; i < last_idx; ++i) { const auto & point = trajectory_msg_->points[i]; const auto & next_point = trajectory_msg_->points[i + 1]; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 130f602c59..a16ff96d13 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -971,7 +971,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN - for (auto i = 0u; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { joint_pos_[i] = std::numeric_limits::quiet_NaN(); joint_vel_[i] = std::numeric_limits::quiet_NaN(); @@ -981,7 +981,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - for (auto i = 0u; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); @@ -1022,7 +1022,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN - for (auto i = 0u; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { joint_pos_[i] = 3.1 + i; joint_vel_[i] = 0.25 + i; @@ -1032,7 +1032,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); - for (auto i = 0u; i < 3; ++i) + for (size_t i = 0; i < 3; ++i) { EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); From 0469455c8fb70f001184e4e11034f271e019273c Mon Sep 17 00:00:00 2001 From: Josh Newans Date: Fri, 15 Oct 2021 18:43:30 +1100 Subject: [PATCH 029/524] Fix diff_drive accel limit (#242) (#252) --- diff_drive_controller/src/diff_drive_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e816b26f11..5e8c080f0d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -165,10 +165,11 @@ controller_interface::return_type DiffDriveController::update( last_command_msg->twist.angular.z = 0.0; } - // linear_command and angular_command may be limited further by SpeedLimit, + // command may be limited further by SpeedLimit, // without affecting the stored twist command - double linear_command = last_command_msg->twist.linear.x; - double angular_command = last_command_msg->twist.angular.z; + Twist command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; // Apply (possibly new) multipliers: const auto wheels = wheel_params_; @@ -248,15 +249,14 @@ controller_interface::return_type DiffDriveController::update( angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); previous_commands_.pop(); - previous_commands_.emplace(*last_command_msg); + previous_commands_.emplace(command); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; limited_velocity_command.header.stamp = current_time; - limited_velocity_command.twist.linear.x = linear_command; - limited_velocity_command.twist.angular.z = angular_command; + limited_velocity_command.twist = command.twist; realtime_limited_velocity_publisher_->unlockAndPublish(); } From 37015b463c547b27a64f4b1a9cea75c5a36e4df9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 24 Oct 2021 09:33:52 +0200 Subject: [PATCH 030/524] Fix python version in the format workflow (#261) --- .github/workflows/format.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 4eec522b51..c6ea8465a8 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -17,6 +17,8 @@ jobs: steps: - uses: actions/checkout@v2 - uses: actions/setup-python@v2 + with: + python-version: 3.9.7 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 cppcheck - uses: pre-commit/action@v2.0.3 From 0e2a43a0c0103cda2fb8cc89fc4c01e27a39b821 Mon Sep 17 00:00:00 2001 From: bailaC Date: Mon, 25 Oct 2021 12:44:27 +0530 Subject: [PATCH 031/524] Fixing issue : 114 (#259) * Fixing issue : 114 * Addressing review comments. --- .../src/forward_command_controller.cpp | 26 ++---------------- .../src/joint_trajectory_controller.cpp | 27 +++---------------- 2 files changed, 5 insertions(+), 48 deletions(-) diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index bc725e509f..ca325a0287 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -20,6 +20,7 @@ #include #include +#include "controller_interface/helpers.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" @@ -105,29 +106,6 @@ ForwardCommandController::state_interface_configuration() const controller_interface::interface_configuration_type::NONE}; } -// Fill ordered_interfaces with references to the matching interfaces -// in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, const std::vector & joint_names, - const std::string & interface_type, std::vector> & ordered_interfaces) -{ - for (const auto & joint_name : joint_names) - { - for (auto & command_interface : unordered_interfaces) - { - if ( - (command_interface.get_name() == joint_name) && - (command_interface.get_interface_name() == interface_type)) - { - ordered_interfaces.push_back(std::ref(command_interface)); - } - } - } - - return joint_names.size() == ordered_interfaces.size(); -} - CallbackReturn ForwardCommandController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -135,7 +113,7 @@ CallbackReturn ForwardCommandController::on_activate( // also verify that we *only* have the resources defined in the "points" parameter std::vector> ordered_interfaces; if ( - !get_ordered_interfaces( + !controller_interface::get_ordered_interfaces( command_interfaces_, joint_names_, interface_name_, ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0dd678c354..c5b75eb60d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -26,6 +26,7 @@ #include "angles/angles.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" +#include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" @@ -697,28 +698,6 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::SUCCESS; } -// Fill ordered_interfaces with references to the matching interfaces -// in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, const std::vector & joint_names, - const std::string & interface_type, std::vector> & ordered_interfaces) -{ - for (const auto & joint_name : joint_names) - { - for (auto & interface : unordered_interfaces) - { - if ( - (interface.get_name() == joint_name) && (interface.get_interface_name() == interface_type)) - { - ordered_interfaces.emplace_back(std::ref(interface)); - } - } - } - - return joint_names.size() == ordered_interfaces.size(); -} - CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { // order all joints in the storage @@ -727,7 +706,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); - if (!get_ordered_interfaces( + if (!controller_interface::get_ordered_interfaces( command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( @@ -741,7 +720,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); - if (!get_ordered_interfaces( + if (!controller_interface::get_ordered_interfaces( state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( From c5fe52fb24d5ff5115d3db26cd7155866371d8ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noe=C3=ABl=20Moeskops?= Date: Mon, 25 Oct 2021 09:34:18 +0200 Subject: [PATCH 032/524] Fixed #255 (#256) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fixed #255 * correct include in correct header * Correct order of includes. Co-authored-by: Denis Štogl --- .../include/diff_drive_controller/rolling_mean_accumulator.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp b/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp index c91e4f7eff..cd90eb9d90 100644 --- a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp +++ b/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp @@ -20,6 +20,7 @@ #define DIFF_DRIVE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ #include +#include #include namespace diff_drive_controller From ba3737de06a9f42c6fc239b156648b3f26875d99 Mon Sep 17 00:00:00 2001 From: bailaC Date: Mon, 25 Oct 2021 14:36:08 +0530 Subject: [PATCH 033/524] Fixing issue : 113. (#258) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fixing issue : 113. * Addressing review comments. * Addressing review comments. * Correct formatting and revert joint_group_effort_controller changes. Co-authored-by: Denis Štogl --- diff_drive_controller/CMakeLists.txt | 2 + diff_drive_controller/package.xml | 1 + diff_drive_controller/test/test_common.hpp | 171 ------------------ .../test/test_load_diff_drive_controller.cpp | 5 +- 4 files changed, 5 insertions(+), 174 deletions(-) delete mode 100644 diff_drive_controller/test/test_common.hpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 2afd58bd60..aa43a1a29a 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -60,6 +60,7 @@ install(TARGETS diff_drive_controller if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller test/test_diff_drive_controller.cpp @@ -87,6 +88,7 @@ if(BUILD_TESTING) target_include_directories(test_load_diff_drive_controller PRIVATE include) ament_target_dependencies(test_load_diff_drive_controller controller_manager + ros2_control_test_assets ) ament_add_gmock( diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 7132e90f98..3bfea1005d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -24,6 +24,7 @@ ament_cmake_gmock controller_manager + ros2_control_test_assets ament_cmake diff --git a/diff_drive_controller/test/test_common.hpp b/diff_drive_controller/test/test_common.hpp deleted file mode 100644 index 130b1eafe8..0000000000 --- a/diff_drive_controller/test/test_common.hpp +++ /dev/null @@ -1,171 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_COMMON_HPP_ -#define TEST_COMMON_HPP_ - -namespace diff_drive_controller_testing -{ -constexpr auto diffbot_urdf = - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 1 - - - Gazebo/Red - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 1 - - - Gazebo/Red - - - - Gazebo/Green - - - Gazebo/Purple - - - - test_actuator - - - - - - - - - - test_actuator - - - - - - - - -)"; - -} // namespace diff_drive_controller_testing - -#endif // TEST_COMMON_HPP_ diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 909b68d3cf..20caf46b6f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -17,7 +17,7 @@ #include "controller_manager/controller_manager.hpp" #include "rclcpp/utilities.hpp" -#include "test_common.hpp" +#include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadDiffDriveController, load_controller) { @@ -27,8 +27,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - diff_drive_controller_testing::diffbot_urdf), + std::make_unique(ros2_control_test_assets::diffbot_urdf), executor, "test_controller_manager"); ASSERT_NO_THROW( From d461d0e7340552e093683f9b68cc2a58847e88a2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 25 Oct 2021 18:35:45 +0100 Subject: [PATCH 034/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 11 files changed, 45 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5d8a0ea8dc..822d7624c2 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use common test URDF from descriptions.hpp (`#258 `_) +* Fix header include on Fedora `_ (`#256 `_) +* Fix diff_drive accel limit (`#242 `_) (`#252 `_) +* Contributors: Denis Štogl, Josh Newans, Noeël Moeskops, bailaC + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6e933ac332..a5e160bb0a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 0cc98d97dd..1c11d7fc97 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 +----------- + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 7d17f602a1..e211cb334a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move interface sorting into ControllerInterface (`#259 `_) +* Revise for-loop style (`#254 `_) +* Contributors: bailaC + 1.0.0 (2021-09-29) ------------------ * Reset and test of command buffer for forwarding controllers. (`#246 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c9acf9b2b9..f3d931ce13 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Remove compile warnings. (`#245 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 02abea3aec..f0088a88f8 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e6d452396d..648d490e58 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revise for-loop style (`#254 `_) +* Contributors: bailaC + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 886dc150b3..c53fdc2deb 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move interface sorting into ControllerInterface (`#259 `_) +* Revise for-loop style (`#254 `_) +* Contributors: bailaC + 1.0.0 (2021-09-29) ------------------ * Remove compile warnings. (`#245 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 12e7957197..a040d4e7e8 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b51c5c809e..4d800d7121 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Remove joint_state_controller, use joint_state_broadcaster instead (`#230 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 11a368304b..eaedb4c927 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.0.0 (2021-09-29) ------------------ * Add time and period to update function (`#241 `_) From ce9703ba90883ec0fa9d843f5e5e8613a17d2389 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 25 Oct 2021 18:36:06 +0100 Subject: [PATCH 035/524] 1.1.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 822d7624c2..8e92a65100 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ * Use common test URDF from descriptions.hpp (`#258 `_) * Fix header include on Fedora `_ (`#256 `_) * Fix diff_drive accel limit (`#242 `_) (`#252 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 3bfea1005d..8d4ede1167 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 1.0.0 + 1.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index a5e160bb0a..c053647637 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index dc01f53385..6c97c245bd 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 1.0.0 + 1.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 1c11d7fc97..8da252d35d 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 ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1017afb3e5..35dbe9e3be 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 1.0.0 + 1.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e211cb334a..235a39895b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ * Move interface sorting into ControllerInterface (`#259 `_) * Revise for-loop style (`#254 `_) * Contributors: bailaC diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 63308f672c..42f98263e8 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 1.0.0 + 1.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f3d931ce13..68ffb84ca2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a8e2808687..883532da60 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 1.0.0 + 1.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index f0088a88f8..b8457b6cdb 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index d5400f7820..0bd2e22485 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 1.0.0 + 1.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 648d490e58..0028f8aa96 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ * Revise for-loop style (`#254 `_) * Contributors: bailaC diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index ec493cb6b6..a349c2223b 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 1.0.0 + 1.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c53fdc2deb..b2dd2f1105 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ * Move interface sorting into ControllerInterface (`#259 `_) * Revise for-loop style (`#254 `_) * Contributors: bailaC diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 5a316e491f..31d9195b80 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 1.0.0 + 1.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index a040d4e7e8..c99445ea26 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c9362a9491..10f0bd3858 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 1.0.0 + 1.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4d800d7121..1ed3b2d414 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 181034d9de..cda71f40f0 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 1.0.0 + 1.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index eaedb4c927..6d64fbaf96 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.1.0 (2021-10-25) +------------------ 1.0.0 (2021-09-29) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 749da4f841..cc9d847aa3 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 1.0.0 + 1.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From d86cffa89fdc05f5b59c5af5e8a836511468665b Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 18 Nov 2021 14:14:41 +0100 Subject: [PATCH 036/524] Add velocity feedback option for diff_drive_controller (#260) --- .../diff_drive_controller.hpp | 4 +- .../diff_drive_controller/odometry.hpp | 1 + .../src/diff_drive_controller.cpp | 49 ++++++++---- diff_drive_controller/src/odometry.cpp | 13 ++- .../config/test_diff_drive_controller.yaml | 3 +- .../test/test_diff_drive_controller.cpp | 79 +++++++++++++++++-- 6 files changed, 124 insertions(+), 25 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 38f767dd3b..026d4edccc 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 @@ -88,10 +88,11 @@ class DiffDriveController : public controller_interface::ControllerInterface protected: struct WheelHandle { - std::reference_wrapper position; + std::reference_wrapper feedback; std::reference_wrapper velocity; }; + const char * feedback_type() const; CallbackReturn configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); @@ -115,6 +116,7 @@ class DiffDriveController : public controller_interface::ControllerInterface struct OdometryParams { bool open_loop = false; + bool position_feedback = true; bool enable_odom_tf = true; std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 57af643bf0..eb3bee9ab6 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -36,6 +36,7 @@ class Odometry void init(const rclcpp::Time & time); bool update(double left_pos, double right_pos, const rclcpp::Time & time); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); void resetOdometry(); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5e8c080f0d..77c9a61a85 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -48,6 +48,11 @@ using lifecycle_msgs::msg::State; DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} +const char * DiffDriveController::feedback_type() const +{ + return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; +} + CallbackReturn DiffDriveController::on_init() { try @@ -68,6 +73,7 @@ CallbackReturn DiffDriveController::on_init() auto_declare>("pose_covariance_diagonal", std::vector()); auto_declare>("twist_covariance_diagonal", std::vector()); auto_declare("open_loop", odom_params_.open_loop); + auto_declare("position_feedback", odom_params_.position_feedback); auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); @@ -123,11 +129,11 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons std::vector conf_names; for (const auto & joint_name : left_wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_POSITION); + conf_names.push_back(joint_name + "/" + feedback_type()); } for (const auto & joint_name : right_wheel_names_) { - conf_names.push_back(joint_name + "/" + HW_IF_POSITION); + conf_names.push_back(joint_name + "/" + feedback_type()); } return {interface_configuration_type::INDIVIDUAL, conf_names}; } @@ -183,28 +189,36 @@ controller_interface::return_type DiffDriveController::update( } else { - double left_position_mean = 0.0; - double right_position_mean = 0.0; + double left_feedback_mean = 0.0; + double right_feedback_mean = 0.0; for (size_t index = 0; index < wheels.wheels_per_side; ++index) { - const double left_position = registered_left_wheel_handles_[index].position.get().get_value(); - const double right_position = - registered_right_wheel_handles_[index].position.get().get_value(); + const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); + const double right_feedback = + registered_right_wheel_handles_[index].feedback.get().get_value(); - if (std::isnan(left_position) || std::isnan(right_position)) + if (std::isnan(left_feedback) || std::isnan(right_feedback)) { RCLCPP_ERROR( - logger, "Either the left or right wheel position is invalid for index [%zu]", index); + logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(), + index); return controller_interface::return_type::ERROR; } - left_position_mean += left_position; - right_position_mean += right_position; + left_feedback_mean += left_feedback; + right_feedback_mean += right_feedback; } - left_position_mean /= wheels.wheels_per_side; - right_position_mean /= wheels.wheels_per_side; + left_feedback_mean /= wheels.wheels_per_side; + right_feedback_mean /= wheels.wheels_per_side; - odometry_.update(left_position_mean, right_position_mean, current_time); + if (odom_params_.position_feedback) + { + odometry_.update(left_feedback_mean, right_feedback_mean, current_time); + } + else + { + odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time); + } } tf2::Quaternion orientation; @@ -331,6 +345,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); + odom_params_.position_feedback = node_->get_parameter("position_feedback").as_bool(); odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); cmd_vel_timeout_ = std::chrono::milliseconds{ @@ -586,10 +601,12 @@ CallbackReturn DiffDriveController::configure_side( registered_handles.reserve(wheel_names.size()); for (const auto & wheel_name : wheel_names) { + const auto interface_name = feedback_type(); const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name](const auto & interface) { + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) { return interface.get_name() == wheel_name && - interface.get_interface_name() == HW_IF_POSITION; + interface.get_interface_name() == interface_name; }); if (state_handle == state_interfaces_.cend()) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 34168fb85e..bd718718fe 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -66,10 +66,19 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti left_wheel_old_pos_ = left_wheel_cur_pos; right_wheel_old_pos_ = right_wheel_cur_pos; + updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time); + + return true; +} + +bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + // Compute linear and angular diff: - const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity - const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + const double angular = (left_vel - right_vel) / wheel_separation_; // Integrate odometry: integrateExact(linear, angular); diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index 30e57361bb..a2149eb6bc 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -1,4 +1,4 @@ -diff_drive_controller: +test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] @@ -17,6 +17,7 @@ diff_drive_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + position_feedback: false open_loop: true enable_odom_tf: true diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ca3da08803..ab007759ef 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -129,7 +129,7 @@ class TestDiffDriveController : public ::testing::Test } } - void assignResources() + void assignResourcesPosFeedback() { std::vector state_ifs; state_ifs.emplace_back(left_wheel_pos_state_); @@ -142,6 +142,19 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_vel_state_); + state_ifs.emplace_back(right_wheel_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; @@ -154,6 +167,10 @@ class TestDiffDriveController : public ::testing::Test left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; hardware_interface::StateInterface right_wheel_pos_state_{ right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + hardware_interface::StateInterface left_wheel_vel_state_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::StateInterface right_wheel_vel_state_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; hardware_interface::CommandInterface left_wheel_vel_cmd_{ left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; hardware_interface::CommandInterface right_wheel_vel_cmd_{ @@ -241,21 +258,73 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(TestDiffDriveController, activate_succeeds_with_resources_assigned) +TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); + // We implicitly test that by default position feedback is required controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - assignResources(); + assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } +TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + TEST_F(TestDiffDriveController, cleanup) { const auto ret = controller_->init(controller_name); @@ -272,7 +341,7 @@ TEST_F(TestDiffDriveController, cleanup) executor.add_node(controller_->get_node()->get_node_base_interface()); auto state = controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - assignResources(); + assignResourcesPosFeedback(); state = controller_->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -321,7 +390,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) executor.add_node(controller_->get_node()->get_node_base_interface()); auto state = controller_->configure(); - assignResources(); + assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); From 600ecb6dbf4bb13c6cf5ed7c7bb0e7f7588f46b6 Mon Sep 17 00:00:00 2001 From: Jack Date: Fri, 19 Nov 2021 01:56:31 -0700 Subject: [PATCH 037/524] Forward command controller test update (#273) * removed unnecessary lines and updated comments * fixed pre-commit issues * removed extra part of test --- .../test/test_forward_command_controller.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 47ed8b7f87..d3dbe22c8d 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -74,7 +74,7 @@ void ForwardCommandControllerTest::SetUpController() TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) { SetUpController(); - controller_->get_node()->set_parameter({"interface_name", "dummy"}); + controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'joints' parameter not set ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -83,11 +83,10 @@ TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) { SetUpController(); + controller_->get_node()->set_parameter({"joints", std::vector()}); // configure failed, 'interface_name' parameter not set - controller_->get_node()->set_parameter({"joints", std::vector()}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->get_node()->set_parameter({"interface_name", ""}); } TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) @@ -104,8 +103,6 @@ TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) { SetUpController(); - - // configure failed, 'interface_name' parameter not set controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); controller_->get_node()->set_parameter({"interface_name", ""}); @@ -134,14 +131,6 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - auto result = controller_->get_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); - ASSERT_TRUE(result.successful); - - // activate failed, 'acceleration' is not a registered interface for `joint1` - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) @@ -151,7 +140,7 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) controller_->get_node()->set_parameter({"joints", joint_names_}); controller_->get_node()->set_parameter({"interface_name", "acceleration"}); - // activate failed, 'joint4' not in interfaces + // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -163,6 +152,7 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) controller_->get_node()->set_parameter({"joints", joint_names_}); controller_->get_node()->set_parameter({"interface_name", "position"}); + // activate successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } From b9afbcd74a5263fafa77181187b8ffa8f0696ea8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 29 Nov 2021 11:47:47 +0100 Subject: [PATCH 038/524] [Joint State Broadcaster] Add option to support only specific interfaces on specific joints (#216) * Joint state broadcaster support for specific joint_names and interface_names. * Extend documentation. Co-authored-by: Bence Magyar --- joint_state_broadcaster/doc/userdoc.rst | 21 +- .../joint_state_broadcaster.hpp | 23 ++ .../src/joint_state_broadcaster.cpp | 61 +++- .../test/test_joint_state_broadcaster.cpp | 327 +++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 45 ++- 5 files changed, 447 insertions(+), 30 deletions(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 12749a41bf..c8f35ffb32 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -13,13 +13,32 @@ Broadcasters are not real controllers, and therefore take no commands. Hardware interface type ----------------------- -All available *joint state interfaces* are used by this broadcaster. +By default, all available *joint state interfaces* are used, unless configured otherwise. +In the latter case, resulting interfaces is defined by a "matrix" of interfaces defined by the cross-product of the ``joints`` and ``interfaces`` parameters. +If some requested interfaces are missing, the controller will print a warning about that, but work for other interfaces. +If none of the requested interface are not defined, the controller returns error on activation. Parameters ---------- ``use_local_topics`` + Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. + +``joints`` + + Optional parameter (string array) to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + Joint state broadcaster asks for access to all defined interfaces on all defined joints. + + +``interfaces`` + + Optional parameter (string array) to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``joints`` parameter. + + ``extra_joints`` + Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 175c3610f6..165e5253e6 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -31,6 +31,25 @@ namespace joint_state_broadcaster { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +/** + * \brief Joint State Broadcaster for all or some state in a ros2_control system. + * + * JointStateBroadcaster publishes state interfaces from ros2_control as ROS messages. + * There is a possibility to publish all available states (typical use), or only specific ones. + * The latter is, for example, used when hardware provides multiple measurement sources for some + * of its states, e.g., position. + * If "joints" or "interfaces" parameter is empty, all available states are published. + * + * \param use_local_topics Flag to publish topics in local namespace. + * \param joints Names of the joints to publish. + * \param interfaces Names of interfaces to publish. + * + * Publishes to: + * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement + * (position, velocity, effort). + * - \b dynamic_joint_states (control_msgs::msg::DynamicJointState): Joint states regardless of + * its interface type. + */ class JointStateBroadcaster : public controller_interface::ControllerInterface { public: @@ -63,9 +82,13 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface bool init_joint_data(); void init_joint_state_msg(); void init_dynamic_joint_state_msg(); + bool use_all_available_interfaces() const; protected: + // Optional parameters bool use_local_topics_; + std::vector joints_; + std::vector interfaces_; // For the JointState message, // we store the name of joints with compatible interfaces diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 178a417d54..f5b9086143 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -51,6 +51,8 @@ CallbackReturn JointStateBroadcaster::on_init() try { auto_declare("use_local_topics", false); + auto_declare>("joints", std::vector({})); + auto_declare>("interfaces", std::vector({})); } catch (const std::exception & e) { @@ -71,14 +73,47 @@ JointStateBroadcaster::command_interface_configuration() const controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::ALL}; + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (use_all_available_interfaces()) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::ALL; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & joint : joints_) + { + for (const auto & interface : interfaces_) + { + state_interfaces_config.names.push_back(joint + "/" + interface); + } + } + } + + return state_interfaces_config; } CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); + joints_ = get_node()->get_parameter("joints").as_string_array(); + interfaces_ = get_node()->get_parameter("interfaces").as_string_array(); + + if (use_all_available_interfaces()) + { + RCLCPP_INFO( + node_->get_logger(), + "'joints' or 'interfaces' parameter is empty. " + "All available state interfaces will be published"); + } + else + { + RCLCPP_INFO( + node_->get_logger(), + "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); + } try { @@ -105,12 +140,24 @@ CallbackReturn JointStateBroadcaster::on_activate( { if (!init_joint_data()) { + RCLCPP_ERROR( + node_->get_logger(), "None of requested interfaces exist. Controller will not run."); return CallbackReturn::ERROR; } init_joint_state_msg(); init_dynamic_joint_state_msg(); + if ( + !use_all_available_interfaces() && + state_interfaces_.size() != (joints_.size() * interfaces_.size())) + { + RCLCPP_WARN( + node_->get_logger(), + "Not all requested interfaces exists. " + "Check ControllerManager output for more detailed information."); + } + return CallbackReturn::SUCCESS; } @@ -139,6 +186,11 @@ bool has_any_key( bool JointStateBroadcaster::init_joint_data() { + if (state_interfaces_.empty()) + { + return false; + } + // loop in reverse order, this maintains the order of values at retrieval time for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { @@ -213,6 +265,11 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg() } } +bool JointStateBroadcaster::use_all_available_interfaces() const +{ + return joints_.empty() || interfaces_.empty(); +} + double get_value( const std::unordered_map> & map, const std::string & name, const std::string & interface_name) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 8dc92993a0..97b8ced127 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,21 +69,85 @@ void JointStateBroadcasterTest::SetUp() void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } -void JointStateBroadcasterTest::SetUpStateBroadcaster() +void JointStateBroadcasterTest::SetUpStateBroadcaster( + const std::vector & joint_names, const std::vector & interfaces) +{ + init_broadcaster_and_set_parameters(joint_names, interfaces); + assign_state_interfaces(joint_names, interfaces); +} + +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"); ASSERT_EQ(result, controller_interface::return_type::OK); + state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); + state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces}); +} + +void JointStateBroadcasterTest::assign_state_interfaces( + const std::vector & joint_names, const std::vector & interfaces) +{ std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_2_pos_state_); - state_ifs.emplace_back(joint_3_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); - state_ifs.emplace_back(joint_2_vel_state_); - state_ifs.emplace_back(joint_3_vel_state_); - state_ifs.emplace_back(joint_1_eff_state_); - state_ifs.emplace_back(joint_2_eff_state_); - state_ifs.emplace_back(joint_3_eff_state_); + + if (joint_names.empty() || interfaces.empty()) + { + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_2_pos_state_); + state_ifs.emplace_back(joint_3_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + state_ifs.emplace_back(joint_2_vel_state_); + state_ifs.emplace_back(joint_3_vel_state_); + state_ifs.emplace_back(joint_1_eff_state_); + state_ifs.emplace_back(joint_2_eff_state_); + state_ifs.emplace_back(joint_3_eff_state_); + } + else + { + for (const auto & joint : joint_names) + { + for (const auto & interface : interfaces) + { + if (joint == joint_names_[0] && interface == interface_names_[0]) + { + state_ifs.emplace_back(joint_1_pos_state_); + } + if (joint == joint_names_[1] && interface == interface_names_[0]) + { + state_ifs.emplace_back(joint_2_pos_state_); + } + if (joint == joint_names_[2] && interface == interface_names_[0]) + { + state_ifs.emplace_back(joint_3_pos_state_); + } + if (joint == joint_names_[0] && interface == interface_names_[1]) + { + state_ifs.emplace_back(joint_1_vel_state_); + } + if (joint == joint_names_[1] && interface == interface_names_[1]) + { + state_ifs.emplace_back(joint_2_vel_state_); + } + if (joint == joint_names_[2] && interface == interface_names_[1]) + { + state_ifs.emplace_back(joint_3_vel_state_); + } + if (joint == joint_names_[0] && interface == interface_names_[2]) + { + state_ifs.emplace_back(joint_1_eff_state_); + } + if (joint == joint_names_[1] && interface == interface_names_[2]) + { + state_ifs.emplace_back(joint_2_eff_state_); + } + if (joint == joint_names_[2] && interface == interface_names_[2]) + { + state_ifs.emplace_back(joint_3_eff_state_); + } + } + } + } state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } @@ -125,7 +189,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); } -TEST_F(JointStateBroadcasterTest, ConfigureSuccessTest) +TEST_F(JointStateBroadcasterTest, ActivateTest) { // joint state not initialized yet ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, IsEmpty()); @@ -148,8 +212,6 @@ TEST_F(JointStateBroadcasterTest, ConfigureSuccessTest) ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); const size_t NUM_JOINTS = joint_names_.size(); - const std::vector IF_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; - const size_t NUM_IFS = IF_NAMES.size(); // joint state initialized ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); @@ -159,17 +221,251 @@ TEST_F(JointStateBroadcasterTest, ConfigureSuccessTest) // dynamic joint state initialized ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_IFS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); ASSERT_THAT( state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = {}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) +{ + const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; + const std::vector IF_NAMES = {interface_names_[0]}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + // configure ok + std::cout << "Now will try to configure controller, ..." << std::endl; + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + std::cout << "Now will try to activate controller, ..." << std::endl; + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + std::cout << "After activation, ..." << std::endl; + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + } + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + } + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) +{ + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + } + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray(IF_NAMES)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) +{ + const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; + const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; + + init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + + // assign state with interfaces which are not set in parameters --> We should actually not assign + // anything because CM will also not do that + // assign_state_interfaces(JOINT_NAMES, {interface_names_[2]}); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // is none of requested interfaces do not exist, the controller will not be activated + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) +{ + const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; + const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; + + init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + + // Manually assign existing interfaces --> one we need is missing + std::vector state_ifs; + + state_ifs.emplace_back(joint_1_pos_state_); + // Missing Joint 1 vel state interface + state_ifs.emplace_back(joint_2_pos_state_); + state_ifs.emplace_back(joint_2_vel_state_); + + state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // here a warning output is expected! + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + // velocity for joint 1 should be nan because state interface does not exit + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[0])); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + } + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray({IF_NAMES[0]})); // joint 1 has only pos interface + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); // publishers initialized @@ -333,7 +629,6 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) std::vector all_joint_names = joint_names_; all_joint_names.insert(all_joint_names.end(), extra_joint_names.begin(), extra_joint_names.end()); const size_t NUM_JOINTS = all_joint_names.size(); - const std::vector IF_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; // joint state initialized ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(all_joint_names)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index bffa5294b2..b4fc9e0e73 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -23,11 +23,23 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + // subclassing and friending so we can access member variables class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); - FRIEND_TEST(JointStateBroadcasterTest, ConfigureSuccessTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest); }; @@ -40,7 +52,17 @@ class JointStateBroadcasterTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpStateBroadcaster(); + void SetUpStateBroadcaster( + const std::vector & joint_names = {}, + const std::vector & interfaces = {}); + + void init_broadcaster_and_set_parameters( + const std::vector & joint_names = {}, + const std::vector & interfaces = {}); + + void assign_state_interfaces( + const std::vector & joint_names = {}, + const std::vector & interfaces = {}); void test_published_joint_state_message(const std::string & topic); @@ -49,26 +71,27 @@ class JointStateBroadcasterTest : public ::testing::Test protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + const std::vector interface_names_ = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; std::vector joint_values_ = {1.1, 2.1, 3.1}; hardware_interface::StateInterface joint_1_pos_state_{ - joint_names_[0], "position", &joint_values_[0]}; + joint_names_[0], interface_names_[0], &joint_values_[0]}; hardware_interface::StateInterface joint_2_pos_state_{ - joint_names_[1], "position", &joint_values_[1]}; + joint_names_[1], interface_names_[0], &joint_values_[1]}; hardware_interface::StateInterface joint_3_pos_state_{ - joint_names_[2], "position", &joint_values_[2]}; + joint_names_[2], interface_names_[0], &joint_values_[2]}; hardware_interface::StateInterface joint_1_vel_state_{ - joint_names_[0], "velocity", &joint_values_[0]}; + joint_names_[0], interface_names_[1], &joint_values_[0]}; hardware_interface::StateInterface joint_2_vel_state_{ - joint_names_[1], "velocity", &joint_values_[1]}; + joint_names_[1], interface_names_[1], &joint_values_[1]}; hardware_interface::StateInterface joint_3_vel_state_{ - joint_names_[2], "velocity", &joint_values_[2]}; + joint_names_[2], interface_names_[1], &joint_values_[2]}; hardware_interface::StateInterface joint_1_eff_state_{ - joint_names_[0], "effort", &joint_values_[0]}; + joint_names_[0], interface_names_[2], &joint_values_[0]}; hardware_interface::StateInterface joint_2_eff_state_{ - joint_names_[1], "effort", &joint_values_[1]}; + joint_names_[1], interface_names_[2], &joint_values_[1]}; hardware_interface::StateInterface joint_3_eff_state_{ - joint_names_[2], "effort", &joint_values_[2]}; + joint_names_[2], interface_names_[2], &joint_values_[2]}; std::unique_ptr state_broadcaster_; }; From f055d44176628b74fbc3040a49cdc7c5274db6e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 28 Dec 2021 22:00:32 +0100 Subject: [PATCH 039/524] [Joint State Broadcaster] Add mapping of custom states to standard values in "/joint_state" message (#217) * Joint state broadcaster support for specific joint_names and interface_names. * Added option to map custom values to interface names to standard names in joint_state message. * Document functionality. * Add configuration examples Co-authored-by: Bence Magyar --- joint_state_broadcaster/doc/userdoc.rst | 53 ++++++- .../joint_state_broadcaster.hpp | 6 + .../src/joint_state_broadcaster.cpp | 56 +++++++- .../test/test_joint_state_broadcaster.cpp | 133 +++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 8 ++ 5 files changed, 243 insertions(+), 13 deletions(-) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c8f35ffb32..8a030d9356 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -22,23 +22,68 @@ Parameters ---------- ``use_local_topics`` - Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. ``joints`` - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``interfaces`` parameter. Joint state broadcaster asks for access to all defined interfaces on all defined joints. ``interfaces`` - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``joints`` parameter. ``extra_joints`` - Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. + + +``map_interface_to_joint_state`` + Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. + Usecases: + + 1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. + Typically one would map both values in separate interfaces in the framework. + To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. + + 1. A robot provides multiple measuring techniques for its joint values which results in slightly different values. + Typically one would use separate interface for providing those values in the framework. + Using multiple joint_state_broadcaster instances we could publish and show both in RViz. + + Format (each line is optional): + + .. code-block:: yaml + + map_interface_to_joint_state: + position: + velocity: + effort: + + + Examples: + + .. code-block:: yaml + + map_interface_to_joint_state: + position: kf_estimated_position + + + .. code-block:: yaml + + map_interface_to_joint_state: + velocity: derived_velocity + effort: derived_effort + + + .. code-block:: yaml + + map_interface_to_joint_state: + effort: torque_sensor + + + .. code-block:: yaml + + map_interface_to_joint_state: + effort: current_sensor diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 165e5253e6..2c0ef6547c 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -38,11 +38,16 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * There is a possibility to publish all available states (typical use), or only specific ones. * The latter is, for example, used when hardware provides multiple measurement sources for some * of its states, e.g., position. + * It is possible to define a mapping of measurements + * from different sources stored in custom interfaces to standard dynamic names in JointState + * message. * If "joints" or "interfaces" parameter is empty, all available states are published. * * \param use_local_topics Flag to publish topics in local namespace. * \param joints Names of the joints to publish. * \param interfaces Names of interfaces to publish. + * \param map_interface_to_joint_state.{HW_IF_POSITION|HW_IF_VELOCITY|HW_IF_EFFORT} mapping + * between custom interface names and standard names in sensor_msgs::msg::JointState message. * * Publishes to: * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement @@ -89,6 +94,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface bool use_local_topics_; std::vector joints_; std::vector interfaces_; + std::unordered_map map_interface_to_joint_state_; // For the JointState message, // we store the name of joints with compatible interfaces diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index f5b9086143..dfa39dfe4b 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -53,6 +53,12 @@ CallbackReturn JointStateBroadcaster::on_init() auto_declare("use_local_topics", false); auto_declare>("joints", std::vector({})); auto_declare>("interfaces", std::vector({})); + auto_declare( + std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION); + auto_declare( + std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, HW_IF_VELOCITY); + auto_declare( + std::string("map_interface_to_joint_state.") + HW_IF_EFFORT, HW_IF_EFFORT); } catch (const std::exception & e) { @@ -104,17 +110,45 @@ CallbackReturn JointStateBroadcaster::on_configure( if (use_all_available_interfaces()) { RCLCPP_INFO( - node_->get_logger(), + get_node()->get_logger(), "'joints' or 'interfaces' parameter is empty. " "All available state interfaces will be published"); + joints_.clear(); + interfaces_.clear(); } else { RCLCPP_INFO( - node_->get_logger(), + get_node()->get_logger(), "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); } + auto get_map_interface_parameter = [&](const std::string & interface) { + std::string interface_to_map = + get_node() + ->get_parameter(std::string("map_interface_to_joint_state.") + interface) + .as_string(); + + if (std::find(interfaces_.begin(), interfaces_.end(), interface) != interfaces_.end()) + { + map_interface_to_joint_state_[interface] = interface; + RCLCPP_WARN( + get_node()->get_logger(), + "Mapping from '%s' to interface '%s' will not be done, because '%s' is defined " + "in 'interface' parameter.", + interface_to_map.c_str(), interface.c_str(), interface.c_str()); + } + else + { + map_interface_to_joint_state_[interface_to_map] = interface; + } + }; + + map_interface_to_joint_state_ = {}; + get_map_interface_parameter(HW_IF_POSITION); + get_map_interface_parameter(HW_IF_VELOCITY); + get_map_interface_parameter(HW_IF_EFFORT); + try { const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; @@ -200,7 +234,12 @@ bool JointStateBroadcaster::init_joint_data() name_if_value_mapping_[si->get_name()] = {}; } // add interface name - name_if_value_mapping_[si->get_name()][si->get_interface_name()] = kUninitializedValue; + std::string interface_name = si->get_interface_name(); + if (map_interface_to_joint_state_.count(interface_name) > 0) + { + interface_name = map_interface_to_joint_state_[interface_name]; + } + name_if_value_mapping_[si->get_name()][interface_name] = kUninitializedValue; } // filter state interfaces that have at least one of the joint_states fields, @@ -291,11 +330,16 @@ controller_interface::return_type JointStateBroadcaster::update( { for (const auto & state_interface : state_interfaces_) { - name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = + std::string interface_name = state_interface.get_interface_name(); + if (map_interface_to_joint_state_.count(interface_name) > 0) + { + interface_name = map_interface_to_joint_state_[interface_name]; + } + name_if_value_mapping_[state_interface.get_name()][interface_name] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), - state_interface.get_interface_name().c_str(), state_interface.get_value()); + get_node()->get_logger(), "%s: %f\n", state_interface.get_full_name().c_str(), + state_interface.get_value()); } joint_state_msg_.header.stamp = time; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 97b8ced127..0475de5099 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -145,6 +145,10 @@ void JointStateBroadcasterTest::assign_state_interfaces( { state_ifs.emplace_back(joint_3_eff_state_); } + if (interface == custom_interface_name_) + { + state_ifs.emplace_back(joint_X_custom_state); + } } } } @@ -324,12 +328,9 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); // configure ok - std::cout << "Now will try to configure controller, ..." << std::endl; ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::cout << "Now will try to activate controller, ..." << std::endl; ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - std::cout << "After activation, ..." << std::endl; const size_t NUM_JOINTS = JOINT_NAMES.size(); @@ -473,6 +474,132 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); } +TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) +{ + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {custom_interface_name_}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, SizeIs(0)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(0)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(0)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(0)); + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray(IF_NAMES)); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) +{ + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {custom_interface_name_}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + state_broadcaster_->get_node()->set_parameter( + {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + } + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + } + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray({HW_IF_POSITION})); // mapping to this value + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + +TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) +{ + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {custom_interface_name_}; + SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + + state_broadcaster_->get_node()->set_parameter( + {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // joint state initialized + ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); + ASSERT_EQ(state_broadcaster_->joint_state_msg_.position[0], custom_joint_value_); + ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + } + ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + for (auto i = 0ul; i < NUM_JOINTS; ++i) + { + ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + } + + // dynamic joint state initialized + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + ElementsAreArray({HW_IF_POSITION})); // mapping to this value + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +} + TEST_F(JointStateBroadcasterTest, UpdateTest) { SetUpStateBroadcaster(); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index b4fc9e0e73..63cc7a5483 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -40,6 +40,9 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); + FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping); + FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMapping); + FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate); FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest); }; @@ -72,7 +75,9 @@ class JointStateBroadcasterTest : public ::testing::Test // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; const std::vector interface_names_ = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; + std::string custom_interface_name_ = "measured_position"; std::vector joint_values_ = {1.1, 2.1, 3.1}; + double custom_joint_value_ = 3.5; hardware_interface::StateInterface joint_1_pos_state_{ joint_names_[0], interface_names_[0], &joint_values_[0]}; @@ -93,6 +98,9 @@ class JointStateBroadcasterTest : public ::testing::Test hardware_interface::StateInterface joint_3_eff_state_{ joint_names_[2], interface_names_[2], &joint_values_[2]}; + hardware_interface::StateInterface joint_X_custom_state{ + joint_names_[0], custom_interface_name_, &custom_joint_value_}; + std::unique_ptr state_broadcaster_; }; From 368bd40e0cc5a0d23d41b9da201a011e6922e335 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Dec 2021 08:30:44 +0000 Subject: [PATCH 040/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 8 ++++++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 11 files changed, 43 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8e92a65100..884aa0dd4f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add velocity feedback option for diff_drive_controller (`#260 `_) +* Contributors: Patrick Roncagliolo + 1.1.0 (2021-10-25) ------------------ * Use common test URDF from descriptions.hpp (`#258 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c053647637..2678493746 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8da252d35d..a6420b25a5 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 +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 235a39895b..8bd0d4ee77 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Forward command controller test update (`#273 `_) + * removed unnecessary lines and updated comments + * fixed pre-commit issues + * removed extra part of test +* Contributors: Jack Center + 1.1.0 (2021-10-25) ------------------ * Move interface sorting into ControllerInterface (`#259 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 68ffb84ca2..6645159677 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b8457b6cdb..4d22a906f6 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 0028f8aa96..3aa3a1c22b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Joint State Broadcaster] Add mapping of custom states to standard values in "/joint_state" message (`#217 `_) +* [Joint State Broadcaster] Add option to support only specific interfaces on specific joints (`#216 `_) +* Contributors: Denis Štogl, Bence Magyar + 1.1.0 (2021-10-25) ------------------ * Revise for-loop style (`#254 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index b2dd2f1105..42403118bc 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ * Move interface sorting into ControllerInterface (`#259 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c99445ea26..cac3a040e1 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1ed3b2d414..bcdf49a2c8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6d64fbaf96..6a805c8b49 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.1.0 (2021-10-25) ------------------ From dedb120993c6a95e240c2cf7d7730767ab701296 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Dec 2021 08:31:09 +0000 Subject: [PATCH 041/524] 1.2.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 884aa0dd4f..2967185413 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ * Add velocity feedback option for diff_drive_controller (`#260 `_) * Contributors: Patrick Roncagliolo diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8d4ede1167..da90076c95 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 1.1.0 + 1.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 2678493746..95aeb12aee 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6c97c245bd..ed836d491c 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 1.1.0 + 1.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a6420b25a5..2c244fdc3a 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 ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 35dbe9e3be..40a0f3ff67 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 1.1.0 + 1.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 8bd0d4ee77..54e5d8991d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ * Forward command controller test update (`#273 `_) * removed unnecessary lines and updated comments * fixed pre-commit issues diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 42f98263e8..0f6e458197 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 1.1.0 + 1.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6645159677..eb74063106 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 883532da60..cb619742e4 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 1.1.0 + 1.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d22a906f6..92d5d33691 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 0bd2e22485..8d35add038 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 1.1.0 + 1.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3aa3a1c22b..99f6459238 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ * [Joint State Broadcaster] Add mapping of custom states to standard values in "/joint_state" message (`#217 `_) * [Joint State Broadcaster] Add option to support only specific interfaces on specific joints (`#216 `_) * Contributors: Denis Štogl, Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a349c2223b..705abb59e7 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 1.1.0 + 1.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 42403118bc..6b6ed685b0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 31d9195b80..7d925406e3 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 1.1.0 + 1.2.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 cac3a040e1..a7c32859a2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 10f0bd3858..64c22924c8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 1.1.0 + 1.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bcdf49a2c8..a7c54372d1 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index cda71f40f0..f24456e58a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 1.1.0 + 1.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6a805c8b49..931bc1af51 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.2.0 (2021-12-29) +------------------ 1.1.0 (2021-10-25) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index cc9d847aa3..1cb35fcb74 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 1.1.0 + 1.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 9a8be063c7fcf4348cc1001df6cd66503ca9e872 Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx <43780894+PaulVerhoeckx@users.noreply.github.com> Date: Wed, 5 Jan 2022 14:31:00 +0100 Subject: [PATCH 042/524] Fix angular velocity direction of diff_drive_controller odometry (#281) Co-authored-by: Paul Verhoeckx --- diff_drive_controller/src/odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index bd718718fe..99bb22890d 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -78,7 +78,7 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp // Compute linear and angular diff: const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity - const double angular = (left_vel - right_vel) / wheel_separation_; + const double angular = (right_vel - left_vel) / wheel_separation_; // Integrate odometry: integrateExact(linear, angular); From a4a3a10140100ac32accda8c5fd2d608a1d31229 Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 6 Jan 2022 13:47:29 +0530 Subject: [PATCH 043/524] Adding reset() for forward_command_controller (#283) --- forward_command_controller/src/forward_command_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index ca325a0287..33fa9bbd34 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -124,7 +124,7 @@ CallbackReturn ForwardCommandController::on_activate( } // reset command buffer if a command came through callback when controller was inactive - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + rt_command_ptr_.reset(); return CallbackReturn::SUCCESS; } @@ -133,7 +133,7 @@ CallbackReturn ForwardCommandController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + rt_command_ptr_.reset(); return CallbackReturn::SUCCESS; } From 9733da06dad536209d514200562ddbb744938c33 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Fri, 7 Jan 2022 02:58:22 -0600 Subject: [PATCH 044/524] ci: :building_construction: add rhel test script to ci (#275) --- .github/workflows/RHEL.yml | 28 ++++++++++++++++++++++++++++ .github/workflows/ci.yml | 23 ++++++++++++----------- 2 files changed, 40 insertions(+), 11 deletions(-) create mode 100644 .github/workflows/RHEL.yml diff --git a/.github/workflows/RHEL.yml b/.github/workflows/RHEL.yml new file mode 100644 index 0000000000..616852357a --- /dev/null +++ b/.github/workflows/RHEL.yml @@ -0,0 +1,28 @@ +name: RHEL Test +on: + pull_request: + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '17 8 * * *' +env: + # if changing this variable, also change jobs.RHEL.container to match + ROS_DISTRO: galactic + +jobs: + RHEL: + name: RHEL test + runs-on: ubuntu-latest + container: jaronl/ros:galactic-alma + steps: + - uses: actions/checkout@v2 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/$ROS_DISTRO/setup.bash + colcon build + colcon test diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 73c1418fb1..bc70ce8262 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -7,35 +7,36 @@ on: schedule: # Run every morning to detect flakiness and broken dependencies - cron: '17 8 * * *' +env: + ROS_DISTRO: galactic jobs: - ci_binary: - name: Galactic binary job + ros_industrial_ci: + name: industrial ci runs-on: ubuntu-latest strategy: matrix: - env: - - {ROS_DISTRO: galactic, ROS_REPO: main} - - {ROS_DISTRO: galactic, ROS_REPO: testing} + ROS_REPO: [main, testing] env: UPSTREAM_WORKSPACE: .github/workspace.repos steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v2 - uses: ros-industrial/industrial_ci@master - env: ${{matrix.env}} + env: + ROS_REPO: ${{ matrix.ROS_REPO }} - ci_source: - name: Galactic source job + ros-tooling-ci: + name: ros-tooling ci runs-on: ubuntu-20.04 strategy: fail-fast: false steps: - uses: ros-tooling/setup-ros@v0.2 with: - required-ros-distributions: galactic + required-ros-distributions: ${{env.ROS_DISTRO}} - uses: ros-tooling/action-ros-ci@v0.2 with: - target-ros2-distro: galactic + target-ros2-distro: ${{env.ROS_DISTRO}} # build all packages listed in the meta package package-name: | diff_drive_controller From c987379c08146eb72204b0a561f15f6b4e021be3 Mon Sep 17 00:00:00 2001 From: Benjamin Hug <61198898+BenjaminHug8@users.noreply.github.com> Date: Mon, 10 Jan 2022 17:36:48 +0100 Subject: [PATCH 045/524] Add publish_rate option for the diff_drive_controller (#278) --- .../diff_drive_controller.hpp | 5 ++ .../src/diff_drive_controller.cpp | 59 +++++++++++-------- 2 files changed, 40 insertions(+), 24 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 026d4edccc..12a9bb75e0 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 @@ -158,6 +158,11 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Time previous_update_timestamp_{0}; + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0}; + bool is_halted = false; bool use_stamped_vel_ = true; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 77c9a61a85..382740d8f4 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -100,6 +100,7 @@ CallbackReturn DiffDriveController::on_init() auto_declare("angular.z.min_acceleration", NAN); auto_declare("angular.z.max_jerk", NAN); auto_declare("angular.z.min_jerk", NAN); + auto_declare("publish_rate", publish_rate_); } catch (const std::exception & e) { @@ -224,32 +225,37 @@ controller_interface::return_type DiffDriveController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (realtime_odometry_publisher_->trylock()) + if (previous_publish_timestamp_ + publish_period_ < current_time) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = current_time; - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); - } + previous_publish_timestamp_ += publish_period_; - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) - { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = current_time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = current_time; + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = current_time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } } const auto update_dt = current_time - previous_update_timestamp_; @@ -464,6 +470,11 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; + // limit the publication on the topics /odom and /tf + publish_rate_ = node_->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + previous_publish_timestamp_ = node_->get_clock()->now(); + // initialize odom values zeros odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); From 58147ef4649305f968de1f7d182558a0cf123996 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 11 Jan 2022 09:09:22 +0000 Subject: [PATCH 046/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 11 files changed, 38 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 2967185413..b628c718e6 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add publish_rate option for the diff_drive_controller (`#278 `_) +* Fix angular velocity direction of diff_drive_controller odometry (`#281 `_) +* Contributors: Benjamin Hug, Paul Verhoeckx + 1.2.0 (2021-12-29) ------------------ * Add velocity feedback option for diff_drive_controller (`#260 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 95aeb12aee..7341688c43 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 2c244fdc3a..dabb83e2ac 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 +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54e5d8991d..5f27e86b6e 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding reset() for forward_command_controller (`#283 `_) +* Contributors: bailaC + 1.2.0 (2021-12-29) ------------------ * Forward command controller test update (`#273 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index eb74063106..3b4f3056cc 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 92d5d33691..57b42ab52f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 99f6459238..08e449a0d1 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ * [Joint State Broadcaster] Add mapping of custom states to standard values in "/joint_state" message (`#217 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 6b6ed685b0..653d20e32d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index a7c32859a2..dc54676450 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a7c54372d1..a225acf4fd 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 931bc1af51..51737724d9 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.2.0 (2021-12-29) ------------------ From d5d24d90615844f04a59a187732adec3aa4efae0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 11 Jan 2022 09:09:39 +0000 Subject: [PATCH 047/524] 1.3.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b628c718e6..4b99f19ade 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ * Add publish_rate option for the diff_drive_controller (`#278 `_) * Fix angular velocity direction of diff_drive_controller odometry (`#281 `_) * Contributors: Benjamin Hug, Paul Verhoeckx diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index da90076c95..fc0a6f0fed 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 1.2.0 + 1.3.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 7341688c43..40b48898f1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ed836d491c..83814a1b1e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 1.2.0 + 1.3.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 dabb83e2ac..07e6a78c08 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 ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 40a0f3ff67..a4815e3139 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 1.2.0 + 1.3.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 5f27e86b6e..d61e7746b3 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ * Adding reset() for forward_command_controller (`#283 `_) * Contributors: bailaC diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0f6e458197..15e094d6f8 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 1.2.0 + 1.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3b4f3056cc..d35c4c6c76 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index cb619742e4..be186fda86 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 1.2.0 + 1.3.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 57b42ab52f..84d383a27a 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 8d35add038..7176c7a6fa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 1.2.0 + 1.3.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 08e449a0d1..5640e228c8 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 705abb59e7..cb1c107ce1 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 1.2.0 + 1.3.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 653d20e32d..cf8cfe170f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 7d925406e3..66af26b5cb 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 1.2.0 + 1.3.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 dc54676450..62b81657dd 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 64c22924c8..b62a46d3ed 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 1.2.0 + 1.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a225acf4fd..183d5d5875 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index f24456e58a..d68cd96faa 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 1.2.0 + 1.3.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 51737724d9..d934a3c89b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +1.3.0 (2022-01-11) +------------------ 1.2.0 (2021-12-29) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 1cb35fcb74..37c388883e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 1.2.0 + 1.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 6c3f74310eea2e8381162a8de4bc9fdc64a668a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 Jan 2022 20:20:39 +0100 Subject: [PATCH 048/524] Update documentation - small fixes (#285) --- doc/controllers_index.rst | 1 + doc/writing_new_controller.rst | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 63eee5b32e..23feac42d7 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -16,6 +16,7 @@ The controllers' namespaces are commanding the following command interface types - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` + - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - ... diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index cf589b86f9..dcf577e131 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -81,7 +81,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. This name defines the controller's type when the controller manager searches for it. - The other two files have to correspond to the definition done in macro at the bottom of the ``.cpp`` file. + The other two parameters have to correspond to the definition done in macro at the bottom of the ``.cpp`` file. 6. **Writing simple test to check if the controller can be found and loaded** From 1fd8bc37f284098aed39d4e6bd324a8528a72ad4 Mon Sep 17 00:00:00 2001 From: livanov93 Date: Wed, 26 Jan 2022 11:49:08 -0700 Subject: [PATCH 049/524] [JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (#239) * Add velocity pid support. * Remove incorrect init test for only velocity command interface. * Add clarification comments for pid aux variables. Adapt update loop. * Change dt for pid to appropriate measure. * Improve partial commands for velocity-only mode. * Extend tests to use velocity-only mode. * Increase timeout for velocity-only mode parametrized tests. * add is_same_sign for better refactor * refactor boolean logic * set velocity to 0.0 on deactivate Co-authored-by: Bence Magyar --- joint_trajectory_controller/CMakeLists.txt | 4 +- .../joint_trajectory_controller.hpp | 8 + .../src/joint_trajectory_controller.cpp | 110 ++++-- .../test/test_trajectory_controller.cpp | 338 +++++++++++------- .../test/test_trajectory_controller_utils.hpp | 45 ++- 5 files changed, 338 insertions(+), 167 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index bcbb95d9a6..b11fdcc657 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(controller_interface REQUIRED) find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -31,6 +32,7 @@ ament_target_dependencies(joint_trajectory_controller builtin_interfaces controller_interface control_msgs + control_toolbox hardware_interface pluginlib rclcpp @@ -65,7 +67,7 @@ if(BUILD_TESTING) ament_add_gtest(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) - set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 180) + set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller joint_trajectory_controller 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 786ef42b0b..f0e74da0c6 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 @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/tolerances.hpp" @@ -134,6 +135,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa InterfaceReferences joint_command_interface_; InterfaceReferences joint_state_interface_; + bool has_position_state_interface_ = false; bool has_velocity_state_interface_ = false; bool has_acceleration_state_interface_ = false; bool has_position_command_interface_ = false; @@ -145,6 +147,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // There should be PID-approach used as in ROS1: // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 bool use_closed_loop_pid_adapter = false; + using PidPtr = std::shared_ptr; + std::vector pids_; + /// Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + /// reserved storage for result of the command when closed loop pid adapter is used + std::vector tmp_command_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c5b75eb60d..4391ae9014 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -107,7 +107,7 @@ JointTrajectoryController::state_interface_configuration() const } controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -197,7 +197,23 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + if (!use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + } + else + { + // Update PIDs + for (auto i = 0ul; i < joint_num; ++i) + { + tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired.positions[i] - state_current.positions[i], + state_desired.velocities[i] - state_current.velocities[i], + (uint64_t)period.nanoseconds()); + } + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } } if (has_acceleration_command_interface_) { @@ -483,18 +499,12 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION)) - { - has_position_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_command_interface_ = true; - } + has_position_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); if (has_velocity_command_interface_) { @@ -502,10 +512,6 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S if (command_interface_types_.size() == 1) { use_closed_loop_pid_adapter = true; - // TODO(anyone): remove this when implemented - RCLCPP_ERROR(logger, "using 'velocity' command interface alone is not yet implemented yet."); - return CallbackReturn::FAILURE; - // if velocity interface can be used without position if multiple defined } else if (!has_position_command_interface_) { @@ -526,6 +532,28 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } + // TODO(livanov93): change when other option is implemented + if (has_velocity_command_interface_ && use_closed_loop_pid_adapter) + { + size_t num_joints = joint_names_.size(); + pids_.resize(num_joints); + ff_velocity_scale_.resize(num_joints); + tmp_command_.resize(num_joints, 0.0); + + // Init PID gains from ROS parameter server + for (size_t i = 0; i < pids_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const auto k_p = auto_declare(prefix + ".p", 0.0); + const auto k_i = auto_declare(prefix + ".i", 0.0); + const auto k_d = auto_declare(prefix + ".d", 0.0); + const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); + // Initialize PID + pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); + } + } + // Read always state interfaces from the parameter because they can be used // independently from the controller's type. // Specialized, child controllers should set its default value. @@ -558,14 +586,12 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_state_interface_ = true; - } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_state_interface_ = true; - } + has_position_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); if (has_velocity_state_interface_) { @@ -774,8 +800,16 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle:: // TODO(anyone): How to halt when using effort commands? for (size_t index = 0; index < joint_names_.size(); ++index) { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + if (has_position_command_interface_) + { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } + + if (has_velocity_command_interface_) + { + joint_command_interface_[1][index].get().set_value(0.0); + } } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -820,6 +854,12 @@ bool JointTrajectoryController::reset() traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); + // reset pids + for (const auto & pid : pids_) + { + pid->reset(); + } + return true; } @@ -965,7 +1005,19 @@ void JointTrajectoryController::fill_partial_goal( for (auto & it : trajectory_msg->points) { // Assume hold position with 0 velocity and acceleration for missing joints - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + if (!it.positions.empty()) + { + if (has_position_command_interface_) + { + // copy last command if cmd interface exists + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + } + else if (has_position_state_interface_) + { + // copy current state if state interface exists + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + } + } if (!it.velocities.empty()) { it.velocities.push_back(0.0); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a16ff96d13..48c4b41e3e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -58,6 +58,8 @@ using test_trajectory_controllers::TestableJointTrajectoryController; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; +bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } + void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure) @@ -267,6 +269,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); + SetPidParameters(); traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -287,8 +290,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); // first update @@ -304,10 +309,19 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong? // Come the flackiness here? const auto allowed_delta = 0.11; // 0.05; + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + } // cleanup state = traj_controller_->cleanup(); @@ -447,6 +461,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].positions[1] = 3.0; traj_msg.points[0].positions[2] = 1.0; + if (traj_controller_->has_velocity_command_interface()) + { + traj_msg.points[0].velocities.resize(3); + traj_msg.points[0].velocities[0] = -0.1; + traj_msg.points[0].velocities[1] = -0.1; + traj_msg.points[0].velocities[2] = -0.1; + } + trajectory_publisher_->publish(traj_msg); } @@ -456,9 +478,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // Currently COMMON_THRESHOLD is adjusted. updateController(rclcpp::Duration::from_seconds(0.25)); - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } } /** @@ -471,6 +503,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; trajectory_msgs::msg::JointTrajectory traj_msg; @@ -485,8 +519,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = 2.0; - traj_msg.points[0].velocities[1] = 1.0; + traj_msg.points[0].velocities[0] = + copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + traj_msg.points[0].velocities[1] = + copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); trajectory_publisher_->publish(traj_msg); } @@ -496,18 +532,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) updateController(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "Joint 3 command should be current position"; + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "Joint 3 command should be current position"; + } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != command_interface_types_.end()) { - // TODO(anyone): need help here - we should at least estimate the sign of the velocity - // EXPECT_NEAR(traj_msg.points[0].velocities[1], joint_vel_[0], threshold); - // EXPECT_NEAR(traj_msg.points[0].velocities[0], joint_vel_[1], threshold); + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -664,24 +705,33 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) subscribeToState(); std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; + std::vector> points_partial_new_velocities{{0.15}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; + expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory // Denis: delta was 0.1 with 0.2 works for me waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory"); - publish(time_from_start, points_partial_new); + points_partial_new_velocities[0][0] = + copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal expected_desired.positions[0] = points_partial_new[0][0]; expected_desired.positions[1] = points_old[0][1]; expected_desired.positions[2] = points_old[0][2]; + expected_desired.velocities[0] = points_partial_new_velocities[0][0]; + expected_desired.velocities[1] = 0.0; + expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -764,16 +814,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur traj_controller_->activate(); std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; std::vector> partial_traj{ {{-1., -2.}, { -2., -4, }}}; + std::vector> partial_traj_velocities{ + {{-0.1, -0.2}, + { + -0.2, + -0.4, + }}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; // Send full trajectory - publish(points_delay, full_traj); + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); // Sleep until first waypoint of full trajectory trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; @@ -784,7 +841,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); - publish(points_delay, partial_traj, rclcpp::Clock().now() + delay * 2); + publish( + points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); // Wait until the end start and end of partial traj expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; @@ -803,7 +861,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; double state_from_command_offset = 0.3; // send msg @@ -811,69 +871,72 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } executor.cancel(); } @@ -899,65 +962,68 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); - // One the first update(s) there **should not** be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + // One the first update(s) there **should not** be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // There should not be a jump toward commands + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } executor.cancel(); } @@ -1104,6 +1170,16 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); +// only velocity controller +INSTANTIATE_TEST_CASE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { @@ -1131,10 +1207,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort", "position"}; set_parameter_and_check_result(); - // command interfaces: velocity alone not yet implemented - command_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - // command interfaces: velocity - position not present command_interface_types_ = {"velocity", "acceleration"}; set_parameter_and_check_result(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8ece5c393a..167f82aaad 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -94,6 +94,10 @@ class TestableJointTrajectoryController return last_commanded_state_; } + bool has_position_command_interface() { return has_position_command_interface_; } + + bool has_velocity_command_interface() { return has_velocity_command_interface_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -146,13 +150,27 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } + void SetPidParameters() + { + auto node = traj_controller_->get_node(); + + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_i(prefix + ".i", 0.0); + const rclcpp::Parameter k_d(prefix + ".d", 0.0); + const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); + const rclcpp::Parameter ff_velocity_scale("ff_velocity_scale/" + joint_names_[i], 1.0); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + } + } void SetUpAndActivateTrajectoryController( bool use_local_parameters = true, const std::vector & parameters = {}, rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); for (const auto & param : parameters) { @@ -167,6 +185,9 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); traj_node_->set_parameter(stopped_velocity_parameters); + // set pid parameters before configure + SetPidParameters(); + traj_controller_->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -249,7 +270,8 @@ class TrajectoryControllerTest : public ::testing::Test void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, const std::vector> & points, rclcpp::Time start_time = rclcpp::Time(), - const std::vector & joint_names = {}) + const std::vector & joint_names = {}, + const std::vector> & points_velocities = {}) { int wait_count = 0; const auto topic = trajectory_publisher_->get_topic_name(); @@ -294,6 +316,15 @@ class TrajectoryControllerTest : public ::testing::Test duration_total = duration_total + duration; } + for (size_t index = 0; index < points_velocities.size(); ++index) + { + traj_msg.points[index].velocities.resize(points_velocities[index].size()); + for (size_t j = 0; j < points_velocities[index].size(); ++j) + { + traj_msg.points[index].velocities[j] = points_velocities[index][j]; + } + } + trajectory_publisher_->publish(traj_msg); } @@ -326,14 +357,20 @@ class TrajectoryControllerTest : public ::testing::Test { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + } } for (size_t i = 0; i < expected_desired.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + } } } From 41c21ee097ad345d611d9f83e6b15434b79c3ebb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 28 Jan 2022 12:42:02 +0000 Subject: [PATCH 050/524] Update changelogs --- 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 | 15 +++++++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 11 files changed, 45 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4b99f19ade..e9e428325f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ * Add publish_rate option for the diff_drive_controller (`#278 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 40b48898f1..83fee5e4e3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 07e6a78c08..d0ee1c7751 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 +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d61e7746b3..8a4facb98f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ * Adding reset() for forward_command_controller (`#283 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d35c4c6c76..d884710536 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 84d383a27a..c05dcdebd2 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5640e228c8..b9cfb92ad9 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index cf8cfe170f..61a4f4b490 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (`#239 `_) + * Add velocity pid support. + * Remove incorrect init test for only velocity command interface. + * Add clarification comments for pid aux variables. Adapt update loop. + * Change dt for pid to appropriate measure. + * Improve partial commands for velocity-only mode. + * Extend tests to use velocity-only mode. + * Increase timeout for velocity-only mode parametrized tests. + * add is_same_sign for better refactor + * refactor boolean logic + * set velocity to 0.0 on deactivate +* Contributors: Lovro Ivanov, Bence Magyar + 1.3.0 (2022-01-11) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 62b81657dd..05901a4198 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 183d5d5875..6f76d65ad2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index d934a3c89b..69332c355a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 1.3.0 (2022-01-11) ------------------ From 85ffedffe3bb9f60eafecdf4f7a9053e580aacfa Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 28 Jan 2022 12:42:22 +0000 Subject: [PATCH 051/524] 2.0.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e9e428325f..4538e2042a 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index fc0a6f0fed..96b7fce07e 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 1.3.0 + 2.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 83fee5e4e3..34f6c64e88 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 83814a1b1e..21bf21da4d 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 1.3.0 + 2.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d0ee1c7751..f639d693ab 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index a4815e3139..42b8e89d4c 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 1.3.0 + 2.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 8a4facb98f..889ec8653a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 15e094d6f8..0d852a0410 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 1.3.0 + 2.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d884710536..e489c82e0f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index be186fda86..a809cbf252 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 1.3.0 + 2.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c05dcdebd2..19f203fc82 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 7176c7a6fa..46f0e59026 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 1.3.0 + 2.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index b9cfb92ad9..773e519e66 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index cb1c107ce1..9de2dbbdc4 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 1.3.0 + 2.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 61a4f4b490..d6918a0557 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ * [JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (`#239 `_) * Add velocity pid support. * Remove incorrect init test for only velocity command interface. diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 66af26b5cb..9f12a5e8ef 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 1.3.0 + 2.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 05901a4198..9c56c1ca4b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b62a46d3ed..2079d49952 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 1.3.0 + 2.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6f76d65ad2..17d4a2c761 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d68cd96faa..259af9768f 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 1.3.0 + 2.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 69332c355a..1318d418a3 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.0 (2022-01-28) +------------------ 1.3.0 (2022-01-11) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 37c388883e..fece09d6fc 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 1.3.0 + 2.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From af34204fb44db7d12557bd7092a0923358917fdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 29 Jan 2022 16:58:54 +0100 Subject: [PATCH 052/524] Update package.xml (#291) --- joint_trajectory_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9f12a5e8ef..262b602fc9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -21,6 +21,7 @@ controller_interface control_msgs + control_toolbox hardware_interface rclcpp rclcpp_lifecycle From a5e103f20be0ba19a9c559081abd16f00368f78b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 1 Feb 2022 08:33:09 +0000 Subject: [PATCH 053/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 11 files changed, 35 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4538e2042a..95650a3d60 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 34f6c64e88..32444c5381 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index f639d693ab..7ffd4322f7 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 889ec8653a..2c4e525e33 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e489c82e0f..a28901a6aa 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 19f203fc82..da4e3c3dba 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 773e519e66..3cc4cdf333 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index d6918a0557..31083b7070 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix missing control_toolbox dependency (`#291 `_) +* Contributors: Denis Štogl + 2.0.0 (2022-01-28) ------------------ * [JointTrajectoryController] Add velocity-only command option for JTC with closed loop controller (`#239 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 9c56c1ca4b..2a99e4367a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 17d4a2c761..76d3645391 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 1318d418a3..632c3b5329 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.0 (2022-01-28) ------------------ From fdfab1e0f8892230883ed1032f7c4b87fd156dfd Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 1 Feb 2022 08:33:22 +0000 Subject: [PATCH 054/524] 2.0.1 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 22 files changed, 33 insertions(+), 33 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 95650a3d60..03504f0e76 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 96b7fce07e..8919466789 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.0.0 + 2.0.1 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 32444c5381..c08511e287 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 21bf21da4d..7f2f0d711a 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.0.0 + 2.0.1 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 7ffd4322f7..f8abc148ca 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 42b8e89d4c..7a4a87e24e 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.0.0 + 2.0.1 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 2c4e525e33..d3ce76b409 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0d852a0410..4cd2264728 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.0.0 + 2.0.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a28901a6aa..a35ab85dcb 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a809cbf252..99c03ccfb6 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.0.0 + 2.0.1 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index da4e3c3dba..a4e321f8c6 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 46f0e59026..f9dc5d31f1 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.0.0 + 2.0.1 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 3cc4cdf333..a9cb45e057 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9de2dbbdc4..fea5614b14 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.0.0 + 2.0.1 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 31083b7070..16e162ff59 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ * Fix missing control_toolbox dependency (`#291 `_) * Contributors: Denis Štogl diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 262b602fc9..dc2f20eea7 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.0.0 + 2.0.1 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 2a99e4367a..d0b4e3ae34 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2079d49952..07034663de 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.0.0 + 2.0.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 76d3645391..e83c36d071 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 259af9768f..906299a56d 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.0.0 + 2.0.1 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 632c3b5329..2cafd36fdf 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.0.1 (2022-02-01) +------------------ 2.0.0 (2022-01-28) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index fece09d6fc..6dafb5167b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.0.0 + 2.0.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 953fec60ea92aadeffb4421991d0435bc8f364c6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 1 Feb 2022 10:47:09 +0000 Subject: [PATCH 055/524] joint_state_broadcaster to use realtime tools (#276) * Use RealtimePublisher for joint_states * Use RealtimePublisher for dynamic joint states --- joint_state_broadcaster/CMakeLists.txt | 2 + .../joint_state_broadcaster.hpp | 7 +- joint_state_broadcaster/package.xml | 1 + .../src/joint_state_broadcaster.cpp | 78 ++-- .../test/test_joint_state_broadcaster.cpp | 336 ++++++++---------- 5 files changed, 212 insertions(+), 212 deletions(-) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index ee29f9320b..2848617db2 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(controller_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) +find_package(realtime_tools REQUIRED) find_package(sensor_msgs REQUIRED) add_library(joint_state_broadcaster @@ -30,6 +31,7 @@ ament_target_dependencies(joint_state_broadcaster pluginlib rclcpp_lifecycle rcutils + realtime_tools sensor_msgs ) # Causes the visibility macros to use dllexport rather than dllimport, diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 2c0ef6547c..568840ae7a 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,6 +25,7 @@ #include "joint_state_broadcaster/visibility_control.h" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" namespace joint_state_broadcaster @@ -100,14 +101,16 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface // we store the name of joints with compatible interfaces std::vector joint_names_; std::shared_ptr> joint_state_publisher_; - sensor_msgs::msg::JointState joint_state_msg_; + std::shared_ptr> + realtime_joint_state_publisher_; // For the DynamicJointState format, we use a map to buffer values in for easier lookup // This allows to preserve whatever order or names/interfaces were initialized. std::unordered_map> name_if_value_mapping_; std::shared_ptr> dynamic_joint_state_publisher_; - control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; + std::shared_ptr> + realtime_dynamic_joint_state_publisher_; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fea5614b14..075569d051 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -23,6 +23,7 @@ hardware_interface rclcpp_lifecycle sensor_msgs + realtime_tools ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index dfa39dfe4b..272a390152 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -156,9 +156,17 @@ CallbackReturn JointStateBroadcaster::on_configure( joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); + realtime_joint_state_publisher_ = + std::make_shared>( + joint_state_publisher_); + dynamic_joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); + + realtime_dynamic_joint_state_publisher_ = + std::make_shared>( + dynamic_joint_state_publisher_); } catch (const std::exception & e) { @@ -281,26 +289,28 @@ void JointStateBroadcaster::init_joint_state_msg() /// with at least one of these interfaces, the rest are omitted from this message // default initialization for joint state message - joint_state_msg_.name = joint_names_; - joint_state_msg_.position.resize(num_joints, kUninitializedValue); - joint_state_msg_.velocity.resize(num_joints, kUninitializedValue); - joint_state_msg_.effort.resize(num_joints, kUninitializedValue); + auto & joint_state_msg = realtime_joint_state_publisher_->msg_; + joint_state_msg.name = joint_names_; + joint_state_msg.position.resize(num_joints, kUninitializedValue); + joint_state_msg.velocity.resize(num_joints, kUninitializedValue); + joint_state_msg.effort.resize(num_joints, kUninitializedValue); } void JointStateBroadcaster::init_dynamic_joint_state_msg() { + auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; const auto & interfaces_and_values = name_ifv.second; - dynamic_joint_state_msg_.joint_names.push_back(name); + dynamic_joint_state_msg.joint_names.push_back(name); control_msgs::msg::InterfaceValue if_value; for (const auto & interface_and_value : interfaces_and_values) { if_value.interface_names.emplace_back(interface_and_value.first); if_value.values.emplace_back(kUninitializedValue); } - dynamic_joint_state_msg_.interface_values.emplace_back(if_value); + dynamic_joint_state_msg.interface_values.emplace_back(if_value); } } @@ -342,37 +352,45 @@ controller_interface::return_type JointStateBroadcaster::update( state_interface.get_value()); } - joint_state_msg_.header.stamp = time; - dynamic_joint_state_msg_.header.stamp = time; - - // update joint state message and dynamic joint state message - for (size_t i = 0; i < joint_names_.size(); ++i) + if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) { - joint_state_msg_.position[i] = - get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION); - joint_state_msg_.velocity[i] = - get_value(name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY); - joint_state_msg_.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT); + auto & joint_state_msg = realtime_joint_state_publisher_->msg_; + + joint_state_msg.header.stamp = time; + + // update joint state message and dynamic joint state message + for (size_t i = 0; i < joint_names_.size(); ++i) + { + joint_state_msg.position[i] = + get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION); + joint_state_msg.velocity[i] = + get_value(name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY); + joint_state_msg.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT); + } + realtime_joint_state_publisher_->unlockAndPublish(); } - for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg_.joint_names.size(); - ++joint_index) + if (realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock()) { - const auto & name = dynamic_joint_state_msg_.joint_names[joint_index]; - for (size_t interface_index = 0; - interface_index < - dynamic_joint_state_msg_.interface_values[joint_index].interface_names.size(); - ++interface_index) + auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.header.stamp = time; + for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg.joint_names.size(); + ++joint_index) { - const auto & interface_name = - dynamic_joint_state_msg_.interface_values[joint_index].interface_names[interface_index]; - dynamic_joint_state_msg_.interface_values[joint_index].values[interface_index] = - name_if_value_mapping_[name][interface_name]; + const auto & name = dynamic_joint_state_msg.joint_names[joint_index]; + for (size_t interface_index = 0; + interface_index < + dynamic_joint_state_msg.interface_values[joint_index].interface_names.size(); + ++interface_index) + { + const auto & interface_name = + dynamic_joint_state_msg.interface_values[joint_index].interface_names[interface_index]; + dynamic_joint_state_msg.interface_values[joint_index].values[interface_index] = + name_if_value_mapping_[name][interface_name]; + } } + realtime_dynamic_joint_state_publisher_->unlockAndPublish(); } - // publish - joint_state_publisher_->publish(joint_state_msg_); - dynamic_joint_state_publisher_->publish(dynamic_joint_state_msg_); return controller_interface::return_type::OK; } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 0475de5099..1ab4b4323d 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -158,19 +158,9 @@ void JointStateBroadcasterTest::assign_state_interfaces( TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) { - // joint state not initialized yet - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.name.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.position.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.velocity.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.effort.empty()); - - // dynamic joint state not initialized yet - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.joint_names.empty()); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.interface_values.empty()); - // publishers not initialized yet - ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // configure failed ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception); @@ -178,33 +168,13 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) SetUpStateBroadcaster(); // check state remains unchanged - // joint state still not initialized - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.name.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.position.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.velocity.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.effort.empty()); - - // dynamic joint state still not initialized - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.joint_names.empty()); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.interface_values.empty()); - // publishers still not initialized - ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); } TEST_F(JointStateBroadcasterTest, ActivateTest) { - // joint state not initialized yet - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, IsEmpty()); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, IsEmpty()); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, IsEmpty()); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, IsEmpty()); - - // dynamic joint state not initialized yet - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, IsEmpty()); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, IsEmpty()); - // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); @@ -217,30 +187,32 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) const size_t NUM_JOINTS = joint_names_.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + dynamic_joint_state_msg.interface_values[2].interface_names, ElementsAreArray(interface_names_)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); } TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) @@ -256,30 +228,32 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) const size_t NUM_JOINTS = joint_names_.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + dynamic_joint_state_msg.interface_values[2].interface_names, ElementsAreArray(interface_names_)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); } TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) @@ -295,30 +269,32 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) const size_t NUM_JOINTS = joint_names_.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(joint_names_)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(joint_names_)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, + dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(interface_names_)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[2].interface_names, + dynamic_joint_state_msg.interface_values[2].interface_names, ElementsAreArray(interface_names_)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); } TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) @@ -334,35 +310,35 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); } - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); } // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, - ElementsAreArray(IF_NAMES)); + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, - ElementsAreArray(IF_NAMES)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); + dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -378,28 +354,29 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); } // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, - ElementsAreArray(IF_NAMES)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); } TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) @@ -445,33 +422,34 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // velocity for joint 1 should be nan because state interface does not exit - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[0])); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[0])); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); } // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray({IF_NAMES[0]})); // joint 1 has only pos interface ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[1].interface_names, - ElementsAreArray(IF_NAMES)); - - // publishers initialized - ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); + dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); } TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) @@ -488,19 +466,20 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, SizeIs(0)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(0)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(0)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(0)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, SizeIs(0)); + ASSERT_THAT(joint_state_msg.position, SizeIs(0)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(0)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(0)); // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, - ElementsAreArray(IF_NAMES)); + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); // publishers initialized ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); @@ -524,26 +503,28 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); } - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); } // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, + dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray({HW_IF_POSITION})); // mapping to this value // publishers initialized @@ -572,28 +553,30 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_EQ(state_broadcaster_->joint_state_msg_.position[0], custom_joint_value_); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); } - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); for (auto i = 0ul; i < NUM_JOINTS; ++i) { - ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i])); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); } // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS)); - ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT( - state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names, - ElementsAreArray({HW_IF_POSITION})); // mapping to this value + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray({HW_IF_POSITION})); // publishers initialized ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); @@ -728,19 +711,9 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) { - // joint state not initialized yet - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.name.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.position.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.velocity.empty()); - ASSERT_TRUE(state_broadcaster_->joint_state_msg_.effort.empty()); - - // dynamic joint state not initialized yet - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.joint_names.empty()); - ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_msg_.interface_values.empty()); - // publishers not initialized yet - ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); SetUpStateBroadcaster(); @@ -758,11 +731,14 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) const size_t NUM_JOINTS = all_joint_names.size(); // joint state initialized - ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(all_joint_names)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS)); - ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS)); + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS)); + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); } From b74460e6b5eeeb57f64e9037e39ce68ebfc810ff Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 4 Feb 2022 08:48:06 +0000 Subject: [PATCH 056/524] INSTANTIATE_TEST_CASE_P -> INSTANTIATE_TEST_SUITE_P (#293) --- .../test/test_trajectory_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 48c4b41e3e..965e9a5919 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1130,8 +1130,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co executor.cancel(); } -// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P - // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, @@ -1171,7 +1169,7 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"})))); // only velocity controller -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( From 427e86ba28fa53ee44974e7289c7c317ffa8d3d2 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 17 Feb 2022 05:27:56 -0800 Subject: [PATCH 057/524] use rolling mean from rcppmath (#211) * use rolling mean from rcppmath Signed-off-by: Karsten Knese * remove test build * add explicit dependency on rcpputils Co-authored-by: Bence Magyar --- diff_drive_controller/CMakeLists.txt | 10 +-- .../diff_drive_controller/odometry.hpp | 4 +- .../rolling_mean_accumulator.hpp | 67 ------------------- diff_drive_controller/package.xml | 1 + .../test/test_accumulator.cpp | 58 ---------------- 5 files changed, 5 insertions(+), 135 deletions(-) delete mode 100644 diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp delete mode 100644 diff_drive_controller/test/test_accumulator.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index aa43a1a29a..0db84ec8e9 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rcpputils REQUIRED) find_package(realtime_tools REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) @@ -38,6 +39,7 @@ ament_target_dependencies(diff_drive_controller pluginlib rclcpp rclcpp_lifecycle + rcpputils realtime_tools tf2 tf2_msgs @@ -91,14 +93,6 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock( - test_accumulator - test/test_accumulator.cpp - ) - - target_include_directories(test_accumulator PRIVATE include) - ament_target_dependencies(test_accumulator) - endif() ament_export_dependencies( diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index eb3bee9ab6..2adc512f88 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -24,8 +24,8 @@ #include -#include "diff_drive_controller/rolling_mean_accumulator.hpp" #include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" namespace diff_drive_controller { @@ -50,7 +50,7 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = diff_drive_controller::RollingMeanAccumulator; + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp b/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp deleted file mode 100644 index cd90eb9d90..0000000000 --- a/diff_drive_controller/include/diff_drive_controller/rolling_mean_accumulator.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * Author: Víctor López - */ - -#ifndef DIFF_DRIVE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ -#define DIFF_DRIVE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ - -#include -#include -#include - -namespace diff_drive_controller -{ -/** - * \brief Simplification of boost::accumulators::accumulator_set> to avoid dragging boost dependencies - * - * Computes the mean of the last accumulated elements - */ -template -class RollingMeanAccumulator -{ -public: - explicit RollingMeanAccumulator(size_t rolling_window_size) - : buffer_(rolling_window_size, 0.0), next_insert_(0), sum_(0.0), buffer_filled_(false) - { - } - - void accumulate(T val) - { - sum_ -= buffer_[next_insert_]; - sum_ += val; - buffer_[next_insert_] = val; - next_insert_++; - buffer_filled_ |= next_insert_ >= buffer_.size(); - next_insert_ = next_insert_ % buffer_.size(); - } - - T getRollingMean() const - { - size_t valid_data_count = buffer_filled_ * buffer_.size() + !buffer_filled_ * next_insert_; - assert(valid_data_count > 0); - return sum_ / valid_data_count; - } - -private: - std::vector buffer_; - size_t next_insert_; - T sum_; - bool buffer_filled_; -}; -} // namespace diff_drive_controller -#endif // DIFF_DRIVE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8919466789..fd31e1f4d7 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -16,6 +16,7 @@ nav_msgs rclcpp rclcpp_lifecycle + rcpputils realtime_tools tf2 tf2_msgs diff --git a/diff_drive_controller/test/test_accumulator.cpp b/diff_drive_controller/test/test_accumulator.cpp deleted file mode 100644 index 4926974b8e..0000000000 --- a/diff_drive_controller/test/test_accumulator.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifdef _MSC_VER -#define _USE_MATH_DEFINES -#endif -#include - -#include -#include -#include - -TEST(TestAccumulator, test_accumulator) -{ - constexpr double THRESHOLD = 1e-12; - diff_drive_controller::RollingMeanAccumulator accum(4); - - accum.accumulate(1.); - EXPECT_NEAR(1., accum.getRollingMean(), THRESHOLD); - - accum.accumulate(1.); - EXPECT_NEAR(1., accum.getRollingMean(), THRESHOLD); - - accum.accumulate(5.); - EXPECT_NEAR(7. / 3., accum.getRollingMean(), THRESHOLD); - - accum.accumulate(5.); - EXPECT_NEAR(12. / 4., accum.getRollingMean(), THRESHOLD); - - // Start removing old values - accum.accumulate(5.); - EXPECT_NEAR(16. / 4., accum.getRollingMean(), THRESHOLD); - - accum.accumulate(5.); - EXPECT_NEAR(20. / 4., accum.getRollingMean(), THRESHOLD); -} - -TEST(TestAccumulator, spam_accumulator) -{ - constexpr double THRESHOLD = 1e-12; - diff_drive_controller::RollingMeanAccumulator accum(10); - for (int i = 0; i < 10000; ++i) - { - accum.accumulate(M_PI); - EXPECT_NEAR(M_PI, accum.getRollingMean(), THRESHOLD); - } -} From e7d0517fbf1371eda48a9421eb82119b270380e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Feb 2022 12:37:23 +0100 Subject: [PATCH 058/524] Move test nodes from the ros2_control_demos repository. (#294) Co-authored-by: Lovro Ivanov --- .github/workflows/ci.yml | 1 + .github/workflows/lint.yml | 2 + ros2_controllers_test_nodes/package.xml | 22 +++ .../resource/ros2_controllers_test_nodes | 0 .../ros2_controllers_test_nodes/__init__.py | 13 ++ .../publisher_forward_position_controller.py | 79 ++++++++++ .../publisher_joint_trajectory_controller.py | 149 ++++++++++++++++++ ros2_controllers_test_nodes/setup.cfg | 4 + ros2_controllers_test_nodes/setup.py | 57 +++++++ 9 files changed, 327 insertions(+) create mode 100644 ros2_controllers_test_nodes/package.xml create mode 100644 ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py create mode 100644 ros2_controllers_test_nodes/setup.cfg create mode 100644 ros2_controllers_test_nodes/setup.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bc70ce8262..8098874342 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -48,6 +48,7 @@ jobs: joint_trajectory_controller gripper_controllers position_controllers + ros2_controllers_test_nodes velocity_controllers vcs-repo-file-url: | https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/.github/workspace.repos diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 5ac782b9b5..8eec08251f 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -27,6 +27,7 @@ jobs: gripper_controllers position_controllers ros2_controllers + ros2_controllers_test_nodes velocity_controllers ament_lint_cpplint: @@ -52,4 +53,5 @@ jobs: gripper_controllers position_controllers ros2_controllers + ros2_controllers_test_nodes velocity_controllers diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml new file mode 100644 index 0000000000..65ceb874e8 --- /dev/null +++ b/ros2_controllers_test_nodes/package.xml @@ -0,0 +1,22 @@ + + + + ros2_controllers_test_nodes + 0.0.0 + Demo nodes for showing and testing functionalities of the ros2_control framework. + + Denis Štogl + Bence Magyar + + Apache-2.0 + + rclpy + std_msgs + trajectory_msgs + + python3-pytest + + + ament_python + + diff --git a/ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes b/ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py new file mode 100644 index 0000000000..019e363a25 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py @@ -0,0 +1,13 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. 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 new file mode 100644 index 0000000000..60ddd75854 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -0,0 +1,79 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Denis Štogl, Lovro Ivanov +# + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray + + +class PublisherForwardPosition(Node): + def __init__(self): + super().__init__("publisher_forward_position_controller") + # Declare all parameters + self.declare_parameter("controller_name", "forward_position_controller") + self.declare_parameter("wait_sec_between_publish", 5) + self.declare_parameter("goal_names", ["pos1", "pos2"]) + + # Read parameters + controller_name = self.get_parameter("controller_name").value + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + + # Read all positions from parameters + self.goals = [] + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [float(value) for value in goal] + self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "commands" + + self.get_logger().info( + f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ + every {wait_sec_between_publish} s' + ) + + self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = Float64MultiArray() + msg.data = self.goals[self.i] + self.get_logger().info(f'Publishing: "{msg.data}"') + self.publisher_.publish(msg) + self.i += 1 + self.i %= len(self.goals) + + +def main(args=None): + rclpy.init(args=args) + + publisher_forward_position = PublisherForwardPosition() + + rclpy.spin(publisher_forward_position) + publisher_forward_position.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py new file mode 100644 index 0000000000..77e04da356 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -0,0 +1,149 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Denis Štogl, Lovro Ivanov +# + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class PublisherJointTrajectory(Node): + def __init__(self): + super().__init__("publisher_position_trajectory_controller") + # Declare all parameters + self.declare_parameter("controller_name", "position_trajectory_controller") + self.declare_parameter("wait_sec_between_publish", 6) + self.declare_parameter("goal_names", ["pos1", "pos2"]) + self.declare_parameter("joints") + self.declare_parameter("check_starting_point", False) + self.declare_parameter("starting_point_limits") + + # Read parameters + controller_name = self.get_parameter("controller_name").value + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + self.joints = self.get_parameter("joints").value + self.check_starting_point = self.get_parameter("check_starting_point").value + self.starting_point = {} + + if self.joints is None or len(self.joints) == 0: + raise Exception('"joints" parameter is not set!') + + # starting point stuff + if self.check_starting_point: + # declare nested params + for name in self.joints: + param_name_tmp = "starting_point_limits" + "." + name + self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159]) + self.starting_point[name] = self.get_parameter(param_name_tmp).value + + for name in self.joints: + if len(self.starting_point[name]) != 2: + raise Exception('"starting_point" parameter is not set correctly!') + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_state_callback, 10 + ) + # initialize starting point status + self.starting_point_ok = not self.check_starting_point + + self.joint_state_msg_received = False + + # Read all positions from parameters + self.goals = [] + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [] + for value in goal: + float_goal.append(float(value)) + self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "joint_trajectory" + + self.get_logger().info( + 'Publishing {} goals on topic "{}" every {} s'.format( + len(goal_names), publish_topic, wait_sec_between_publish + ) + ) + + self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + + if self.starting_point_ok: + + traj = JointTrajectory() + traj.joint_names = self.joints + point = JointTrajectoryPoint() + point.positions = self.goals[self.i] + point.time_from_start = Duration(sec=4) + + traj.points.append(point) + self.publisher_.publish(traj) + + self.i += 1 + self.i %= len(self.goals) + + elif self.check_starting_point and not self.joint_state_msg_received: + self.get_logger().warn( + 'Start configuration could not be checked! Check "joint_state" topic!' + ) + else: + self.get_logger().warn("Start configuration is not within configured limits!") + + def joint_state_callback(self, msg): + + if not self.joint_state_msg_received: + + # check start state + limit_exceeded = [False] * len(msg.name) + for idx, enum in enumerate(msg.name): + if (msg.position[idx] < self.starting_point[enum][0]) or ( + msg.position[idx] > self.starting_point[enum][1] + ): + self.get_logger().warn(f"Starting point limits exceeded for joint {enum} !") + limit_exceeded[idx] = True + + if any(limit_exceeded): + self.starting_point_ok = False + else: + self.starting_point_ok = True + + self.joint_state_msg_received = True + else: + return + + +def main(args=None): + rclpy.init(args=args) + + publisher_joint_trajectory = PublisherJointTrajectory() + + rclpy.spin(publisher_joint_trajectory) + publisher_joint_trajectory.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros2_controllers_test_nodes/setup.cfg b/ros2_controllers_test_nodes/setup.cfg new file mode 100644 index 0000000000..c00c975bcf --- /dev/null +++ b/ros2_controllers_test_nodes/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/ros2_controllers_test_nodes +[install] +install-scripts=$base/lib/ros2_controllers_test_nodes diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py new file mode 100644 index 0000000000..061e7aef38 --- /dev/null +++ b/ros2_controllers_test_nodes/setup.py @@ -0,0 +1,57 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from glob import glob + +from setuptools import setup + +package_name = "ros2_controllers_test_nodes" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name, glob("launch/*.launch.py")), + ("share/" + package_name + "/configs", glob("configs/*.*")), + ], + install_requires=["setuptools"], + zip_safe=True, + author="Denis Štogl", + author_email="denis@stogl.de", + maintainer="Denis Štogl", + maintainer_email="denis@stogl.de", + keywords=["ROS"], + classifiers=[ + "Intended Audience :: Developers", + "License :: OSI Approved :: Apache Software License", + "Programming Language :: Python", + "Topic :: Software Development", + ], + description="Demo nodes for showing and testing functionalities of ros2_control framework.", + long_description="""\ +Demo nodes for showing and testing functionalities of the ros2_control framework.""", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "publisher_forward_position_controller = \ + ros2_controllers_test_nodes.publisher_forward_position_controller:main", + "publisher_joint_trajectory_controller = \ + ros2_controllers_test_nodes.publisher_joint_trajectory_controller:main", + ], + }, +) From e18d3a04b90cb72f50bb591f6da39031bbbabcbc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 23 Feb 2022 11:54:53 +0000 Subject: [PATCH 059/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 ++ 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 | 5 ++ joint_trajectory_controller/CHANGELOG.rst | 5 ++ position_controllers/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 56 +++++++++++++++++++ velocity_controllers/CHANGELOG.rst | 3 + 12 files changed, 95 insertions(+) create mode 100644 ros2_controllers_test_nodes/CHANGELOG.rst diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 03504f0e76..02912910eb 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* use rolling mean from rcppmath (`#211 `_) +* Contributors: Karsten Knese, Bence Magyar + 2.0.1 (2022-02-01) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c08511e287..c4b338e253 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index f8abc148ca..4e453c4ca5 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d3ce76b409..971f29a6cc 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a35ab85dcb..ad812dbab8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a4e321f8c6..86e21a9ccf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index a9cb45e057..e918ec263c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* joint_state_broadcaster to use realtime tools (`#276 `_) +* Contributors: Bence Magyar + 2.0.1 (2022-02-01) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 16e162ff59..6b3451cb3e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* INSTANTIATE_TEST_CASE_P -> INSTANTIATE_TEST_SUITE_P (`#293 `_) +* Contributors: Bence Magyar + 2.0.1 (2022-02-01) ------------------ * Fix missing control_toolbox dependency (`#291 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d0b4e3ae34..3f141c16f1 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index e83c36d071..5375d603ed 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst new file mode 100644 index 0000000000..eb8b34b702 --- /dev/null +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -0,0 +1,56 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ros2_controllers_test_nodes +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Move test nodes from the ros2_control_demos repository. (`#294 `_) +* Contributors: Denis Štogl, Lovro Ivanov + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 2cafd36fdf..f4a2cb1d4e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.0.1 (2022-02-01) ------------------ From b4e4dc8771bbc19cf81c251d61447caf114414dd Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 23 Feb 2022 11:56:02 +0000 Subject: [PATCH 060/524] Bump package version to upstream --- ros2_controllers_test_nodes/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 65ceb874e8..a08cffe532 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 0.0.0 + 2.0.1 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl From 79e050a05ce83896d3bd28749d4a288cfaf427bd Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 23 Feb 2022 11:57:33 +0000 Subject: [PATCH 061/524] 2.1.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 02912910eb..e136237ebf 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ * use rolling mean from rcppmath (`#211 `_) * Contributors: Karsten Knese, Bence Magyar diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index fd31e1f4d7..d660b2ae1f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.0.1 + 2.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c4b338e253..15b2377aa6 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 7f2f0d711a..90fd24207d 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.0.1 + 2.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4e453c4ca5..b1b16f9fc1 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 7a4a87e24e..7792a77d8a 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.0.1 + 2.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 971f29a6cc..6fc27346b1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 4cd2264728..500b156202 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.0.1 + 2.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ad812dbab8..f3928d96f3 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 99c03ccfb6..6939197808 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.0.1 + 2.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 86e21a9ccf..6eb8f1a9b8 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index f9dc5d31f1..b7c434f88e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.0.1 + 2.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e918ec263c..1c955ec1dd 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ * joint_state_broadcaster to use realtime tools (`#276 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 075569d051..e2fcb35326 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.0.1 + 2.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 6b3451cb3e..1ad1b61e2f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ * INSTANTIATE_TEST_CASE_P -> INSTANTIATE_TEST_SUITE_P (`#293 `_) * Contributors: Bence Magyar diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index dc2f20eea7..eebde38b51 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.0.1 + 2.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 3f141c16f1..3d1d6dfa00 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 07034663de..471702a389 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.0.1 + 2.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5375d603ed..0f689e50db 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 906299a56d..d3a6985cb8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.0.1 + 2.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index eb8b34b702..1a0d314b0e 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ * Move test nodes from the ros2_control_demos repository. (`#294 `_) * Contributors: Denis Štogl, Lovro Ivanov diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index a08cffe532..b3c028e4f6 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.0.1 + 2.1.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f4a2cb1d4e..b4a1247eb2 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.1.0 (2022-02-23) +------------------ 2.0.1 (2022-02-01) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 6dafb5167b..b36eaf3d95 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.0.1 + 2.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 8dd2b5f2a9d78e1cca73c6d566763d03e5f804b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Feb 2022 13:00:22 +0100 Subject: [PATCH 062/524] Adding config for reviewers lottery. (#299) * Adding config for reviewers lottery. Copied from ros2_control repo. * Create reviewer_lottery.yml --- .github/reviewer-lottery.yml | 41 ++++++++++++++++++++++++++ .github/workflows/reviewer_lottery.yml | 13 ++++++++ 2 files changed, 54 insertions(+) create mode 100644 .github/reviewer-lottery.yml create mode 100644 .github/workflows/reviewer_lottery.yml diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml new file mode 100644 index 0000000000..fbaf954340 --- /dev/null +++ b/.github/reviewer-lottery.yml @@ -0,0 +1,41 @@ +groups: + # Default reviewers for all pull requests. + # Usually, at least on of the maintainers should approve PR before merging. + # The best is if two maintainers do that. + - name: maintainers # name of the group + reviewers: 2 # how many reviewers do you want to assign? + internal_reviewers: 1 # how many reviewers do you want to assign when the PR author belongs to this group? + usernames: # github usernames of the reviewers + - bmagyar + - destogl + + # Reviewers group to get broader feedback. + - name: reviewers + reviewers: 5 + usernames: + - rosterloh + - kellyprankin + - progtologist + - arne48 + - DasRoteSkelett + - a10263790 + - Serafadam + - harderthan + - jaron-l + - malapatiravi + - erickisos + - ShawnSchaerer + - Briancbn + - TomoyaFujita2016 + - homalozoa + - anfemosa + - jackcenter + - VX792 + - mhubii + - livanov93 + - aprotyas + - peterdavidfagan + - UsamaHamayun1 + - duringhof + - bijoua29 + - kasiceo diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml new file mode 100644 index 0000000000..f7ae929e5b --- /dev/null +++ b/.github/workflows/reviewer_lottery.yml @@ -0,0 +1,13 @@ +name: Reviewer lottery +on: + pull_request_target: + types: [opened, ready_for_review, reopened] + +jobs: + test: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: uesteibar/reviewer-lottery@v2 + with: + repo-token: ${{ secrets.GITHUB_TOKEN }} From b9ccf04aa95203e37077f297ddcf9f933ef464fa Mon Sep 17 00:00:00 2001 From: "Erick G. Islas-Osuna" Date: Fri, 25 Feb 2022 01:58:30 -0600 Subject: [PATCH 063/524] Remove version from clang-format (#297) * feat: clang-format * clang-format-12 * wip: clang-11 to keep compatibility --- .clang-format | 5 ++--- .github/workflows/ci.yml | 16 ++++++++-------- .github/workflows/format.yml | 18 +++++++++--------- .pre-commit-config.yaml | 20 ++++++++++---------- 4 files changed, 29 insertions(+), 30 deletions(-) diff --git a/.clang-format b/.clang-format index 9c21e1e17c..ab8d077577 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,6 @@ --- -Language: Cpp -BasedOnStyle: Google +Language: Cpp +BasedOnStyle: Google ColumnLimit: 100 AccessModifierOffset: -2 @@ -12,4 +12,3 @@ DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: false IncludeBlocks: Preserve -... diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8098874342..06086cb44f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -6,7 +6,7 @@ on: - master schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '17 8 * * *' + - cron: "17 8 * * *" env: ROS_DISTRO: galactic @@ -15,15 +15,15 @@ jobs: name: industrial ci runs-on: ubuntu-latest strategy: - matrix: - ROS_REPO: [main, testing] + matrix: + ROS_REPO: [main, testing] env: - UPSTREAM_WORKSPACE: .github/workspace.repos + UPSTREAM_WORKSPACE: .github/workspace.repos steps: - - uses: actions/checkout@v2 - - uses: ros-industrial/industrial_ci@master - env: - ROS_REPO: ${{ matrix.ROS_REPO }} + - uses: actions/checkout@v2 + - uses: ros-industrial/industrial_ci@master + env: + ROS_REPO: ${{ matrix.ROS_REPO }} ros-tooling-ci: name: ros-tooling ci diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index c6ea8465a8..9b00d3f4e6 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -15,12 +15,12 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 - with: - python-version: 3.9.7 - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 cppcheck - - uses: pre-commit/action@v2.0.3 - with: - extra_args: --all-files --hook-stage manual + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + with: + python-version: 3.9.7 + - name: Install clang-format + run: sudo apt install -qq clang-format-11 cppcheck + - uses: pre-commit/action@v2.0.3 + with: + extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 70116088ce..f94b77ac76 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,4 +1,3 @@ - # To use: # # pre-commit run -a @@ -36,21 +35,22 @@ repos: - repo: https://github.com/asottile/pyupgrade rev: v2.12.0 hooks: - - id: pyupgrade + - id: pyupgrade args: [--py36-plus] # PEP 257 - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 rev: v0.3.3 hooks: - - id: pep257 - args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - id: pep257 + args: + ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 rev: 3.9.0 hooks: - - id: flake8 - args: ["--ignore=E501"] + - id: flake8 + args: ["--ignore=E501"] # CPP hooks - repo: local @@ -58,10 +58,10 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-11 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'] + args: ["-fallback-style=none", "-i"] - repo: local hooks: @@ -111,7 +111,7 @@ repos: rev: 0.9.0a1 hooks: - id: doc8 - args: ['--max-line-length=100', '--ignore=D001'] + args: ["--max-line-length=100", "--ignore=D001"] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks @@ -128,5 +128,5 @@ repos: rev: v2.0.0 hooks: - id: codespell - args: ['--write-changes'] + args: ["--write-changes"] exclude: CHANGELOG\.rst|\.(svg|pyc)$ From 3e56362f50f9997a54255f4619497ad48310cc7e Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 9 Mar 2022 21:18:44 +0300 Subject: [PATCH 064/524] Export dependency (#310) --- joint_trajectory_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index b11fdcc657..17e67b1f73 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -115,6 +115,7 @@ endif() ament_export_dependencies( controller_interface control_msgs + control_toolbox hardware_interface rclcpp rclcpp_lifecycle From b201524d7451a6df3b954c20c623c4e604bcf971 Mon Sep 17 00:00:00 2001 From: "Erick G. Islas-Osuna" Date: Sat, 12 Mar 2022 10:05:12 -0600 Subject: [PATCH 065/524] Use time argument on update function instead of node time (#296) --- .../src/joint_trajectory_controller.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4391ae9014..9a43d9195e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -107,7 +107,7 @@ JointTrajectoryController::state_interface_configuration() const } controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) + const rclcpp::Time & time, const rclcpp::Duration & period) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -148,10 +148,10 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = - [&, joint_num](auto & joint_inteface, const std::vector & trajectory_point_interface) { + [&, joint_num](auto & joint_interface, const std::vector & trajectory_point_interface) { for (size_t index = 0; index < joint_num; ++index) { - joint_inteface[index].get().set_value(trajectory_point_interface[index]); + joint_interface[index].get().set_value(trajectory_point_interface[index]); } }; @@ -167,12 +167,11 @@ controller_interface::return_type JointTrajectoryController::update( { if (open_loop_control_) { - (*traj_point_active_ptr_) - ->set_point_before_trajectory_msg(node_->now(), last_commanded_state_); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(node_->now(), state_current); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current); } } resize_joint_trajectory_point(state_error, joint_num); @@ -181,8 +180,7 @@ controller_interface::return_type JointTrajectoryController::update( TrajectoryPointConstIter start_segment_itr, end_segment_itr; // TODO(anyone): this is kind-of open-loop concept? I am right? const bool valid_point = - (*traj_point_active_ptr_) - ->sample(node_->now(), state_desired, start_segment_itr, end_segment_itr); + (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr); if (valid_point) { @@ -254,7 +252,7 @@ controller_interface::return_type JointTrajectoryController::update( { // send feedback auto feedback = std::make_shared(); - feedback->header.stamp = node_->now(); + feedback->header.stamp = time; feedback->joint_names = joint_names_; feedback->actual = state_current; @@ -303,7 +301,7 @@ controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - const double difference = node_->now().seconds() - traj_end.seconds(); + const double difference = time.seconds() - traj_end.seconds(); if (difference > default_tolerances_.goal_time_tolerance) { auto result = std::make_shared(); @@ -330,10 +328,10 @@ void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & { const auto joint_num = joint_names_.size(); auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { + [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { for (size_t index = 0; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; @@ -369,10 +367,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto const auto joint_num = joint_names_.size(); auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_inteface) { + [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { for (size_t index = 0; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; From bfe9cc0c461725abfc78e260bd94b1f291e916b3 Mon Sep 17 00:00:00 2001 From: DasRoteSkelett Date: Sat, 19 Mar 2022 13:24:33 +0100 Subject: [PATCH 066/524] JointTrajectoryController: added missing control_toolbox dependencies (#315) Signed-off-by: Matthias Schoepfer --- joint_trajectory_controller/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 17e67b1f73..5a1836573f 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -77,6 +77,7 @@ if(BUILD_TESTING) hardware_interface rclcpp rclcpp_lifecycle + control_toolbox realtime_tools ros2_control_test_assets trajectory_msgs @@ -89,6 +90,7 @@ if(BUILD_TESTING) target_include_directories(test_load_joint_trajectory_controller PRIVATE include) ament_target_dependencies(test_load_joint_trajectory_controller controller_manager + control_toolbox realtime_tools ros2_control_test_assets ) From 654bf1d954b66668d40db70cc5a0eb8bed7d47eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 25 Mar 2022 11:33:35 +0100 Subject: [PATCH 067/524] Updating CI configs and fixing up workflows (#317) * Adding CI config for rolling binary and semi-binary builds. Fixup also other workflows. * Update ci-coverage-build.yml * Create rolling-abi-compatibility-last-focal.yml * Update galactic-source-build.yml * Update rolling-source-build.yml * Delete workspace.repos * Update ci-coverage-build.yml * Delete galactic-rhel-binary-build.yml * Rolling on 22.04 ignore PR execution * Rolling on 22.04 ignore PR executio * Update rolling-semi-binary-build.yml * Update README.md * Re-enable jammy/rolling execution because it works now. * Correct branch link Co-authored-by: Peter David Fagan Co-authored-by: Bence Magyar --- .github/workflows/RHEL.yml | 28 ----- .../{ci.yml => ci-coverage-build.yml} | 52 ++++----- .github/workflows/ci-format.yml | 26 +++++ .../workflows/{lint.yml => ci-ros-lint.yml} | 0 .github/workflows/format.yml | 26 ----- .github/workflows/foxy-abi-compatibility.yml | 17 +++ .github/workflows/foxy-binary-build.yml | 21 ++++ .github/workflows/foxy-semi-binary-build.yml | 21 ++++ .../workflows/galactic-abi-compatibility.yml | 17 +++ .github/workflows/galactic-binary-build.yml | 21 ++++ .../workflows/galactic-semi-binary-build.yml | 21 ++++ .github/workflows/galactic-source-build.yml | 15 +++ .github/workflows/prerelease-check.yml | 39 +++++++ .github/workflows/prerelease.yaml | 26 ----- .../reusable-industrial-ci-with-cache.yml | 106 ++++++++++++++++++ .../reusable-ros-tooling-source-build.yml | 53 +++++++++ .../rolling-abi-compatibility-last-focal.yml | 21 ++++ .../workflows/rolling-abi-compatibility.yml | 17 +++ .../rolling-binary-build-last-focal.yml | 26 +++++ .github/workflows/rolling-binary-build.yml | 21 ++++ .../workflows/rolling-rhel-binary-build.yml | 30 +++++ .../rolling-semi-binary-build-last-focal.yml | 26 +++++ .../workflows/rolling-semi-binary-build.yml | 21 ++++ .github/workflows/rolling-source-build.yml | 15 +++ .pre-commit-config.yaml | 12 +- README.md | 30 ++++- ros2_controllers-not-released.foxy.repos | 1 + ros2_controllers-not-released.galactic.repos | 1 + ros2_controllers-not-released.rolling.repos | 1 + ros2_controllers.foxy.repos | 17 +++ ros2_controllers.galactic.repos | 17 +++ ...ce.repos => ros2_controllers.rolling.repos | 0 32 files changed, 624 insertions(+), 121 deletions(-) delete mode 100644 .github/workflows/RHEL.yml rename .github/workflows/{ci.yml => ci-coverage-build.yml} (54%) create mode 100644 .github/workflows/ci-format.yml rename .github/workflows/{lint.yml => ci-ros-lint.yml} (100%) delete mode 100644 .github/workflows/format.yml create mode 100644 .github/workflows/foxy-abi-compatibility.yml create mode 100644 .github/workflows/foxy-binary-build.yml create mode 100644 .github/workflows/foxy-semi-binary-build.yml create mode 100644 .github/workflows/galactic-abi-compatibility.yml create mode 100644 .github/workflows/galactic-binary-build.yml create mode 100644 .github/workflows/galactic-semi-binary-build.yml create mode 100644 .github/workflows/galactic-source-build.yml create mode 100644 .github/workflows/prerelease-check.yml delete mode 100644 .github/workflows/prerelease.yaml create mode 100644 .github/workflows/reusable-industrial-ci-with-cache.yml create mode 100644 .github/workflows/reusable-ros-tooling-source-build.yml create mode 100644 .github/workflows/rolling-abi-compatibility-last-focal.yml create mode 100644 .github/workflows/rolling-abi-compatibility.yml create mode 100644 .github/workflows/rolling-binary-build-last-focal.yml create mode 100644 .github/workflows/rolling-binary-build.yml create mode 100644 .github/workflows/rolling-rhel-binary-build.yml create mode 100644 .github/workflows/rolling-semi-binary-build-last-focal.yml create mode 100644 .github/workflows/rolling-semi-binary-build.yml create mode 100644 .github/workflows/rolling-source-build.yml create mode 100644 ros2_controllers-not-released.foxy.repos create mode 100644 ros2_controllers-not-released.galactic.repos create mode 100644 ros2_controllers-not-released.rolling.repos create mode 100644 ros2_controllers.foxy.repos create mode 100644 ros2_controllers.galactic.repos rename .github/workspace.repos => ros2_controllers.rolling.repos (100%) diff --git a/.github/workflows/RHEL.yml b/.github/workflows/RHEL.yml deleted file mode 100644 index 616852357a..0000000000 --- a/.github/workflows/RHEL.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: RHEL Test -on: - pull_request: - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '17 8 * * *' -env: - # if changing this variable, also change jobs.RHEL.container to match - ROS_DISTRO: galactic - -jobs: - RHEL: - name: RHEL test - runs-on: ubuntu-latest - container: jaronl/ros:galactic-alma - steps: - - uses: actions/checkout@v2 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/$ROS_DISTRO/setup.bash - colcon build - colcon test diff --git a/.github/workflows/ci.yml b/.github/workflows/ci-coverage-build.yml similarity index 54% rename from .github/workflows/ci.yml rename to .github/workflows/ci-coverage-build.yml index 06086cb44f..8176bbdf5c 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -1,65 +1,55 @@ -name: Test ros2_controllers +name: Coverage Build on: pull_request: - push: branches: - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: "17 8 * * *" -env: - ROS_DISTRO: galactic jobs: - ros_industrial_ci: - name: industrial ci - runs-on: ubuntu-latest - strategy: - matrix: - ROS_REPO: [main, testing] - env: - UPSTREAM_WORKSPACE: .github/workspace.repos - steps: - - uses: actions/checkout@v2 - - uses: ros-industrial/industrial_ci@master - env: - ROS_REPO: ${{ matrix.ROS_REPO }} - - ros-tooling-ci: - name: ros-tooling ci + coverage: + name: coverage build runs-on: ubuntu-20.04 strategy: fail-fast: false + env: + ROS_DISTRO: rolling steps: - uses: ros-tooling/setup-ros@v0.2 with: - required-ros-distributions: ${{env.ROS_DISTRO}} + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: actions/checkout@v3 - uses: ros-tooling/action-ros-ci@v0.2 with: - target-ros2-distro: ${{env.ROS_DISTRO}} + target-ros2-distro: ${{ env.ROS_DISTRO }} + import-token: ${{ secrets.GITHUB_TOKEN }} # build all packages listed in the meta package - package-name: | + package-name: diff_drive_controller effort_controllers force_torque_sensor_broadcaster forward_command_controller - imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller gripper_controllers position_controllers + ros2_controllers ros2_controllers_test_nodes velocity_controllers vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/.github/workspace.repos - colcon-mixin-name: coverage-gcc + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ 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@v1 + skip-tests: true + - uses: codecov/codecov-action@v1.0.14 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - uses: actions/upload-artifact@v1 with: - name: colcon-logs-${{ matrix.os }} + name: colcon-logs path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml new file mode 100644 index 0000000000..dffa28d1c0 --- /dev/null +++ b/.github/workflows/ci-format.yml @@ -0,0 +1,26 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + push: + branches: + - master + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v2 + with: + python-version: 3.9.7 + - name: Install system hooks + run: sudo apt install -qq clang-format-11 cppcheck + - uses: pre-commit/action@v2.0.3 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/lint.yml b/.github/workflows/ci-ros-lint.yml similarity index 100% rename from .github/workflows/lint.yml rename to .github/workflows/ci-ros-lint.yml diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml deleted file mode 100644 index 9b00d3f4e6..0000000000 --- a/.github/workflows/format.yml +++ /dev/null @@ -1,26 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - workflow_dispatch: - pull_request: - push: - branches: - - master - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 - with: - python-version: 3.9.7 - - name: Install clang-format - run: sudo apt install -qq clang-format-11 cppcheck - - uses: pre-commit/action@v2.0.3 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml new file mode 100644 index 0000000000..40b1e1133f --- /dev/null +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -0,0 +1,17 @@ +name: ABI Compatibility Check +on: + pull_request: + branches: + - foxy + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: foxy + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build.yml b/.github/workflows/foxy-binary-build.yml new file mode 100644 index 0000000000..bd3050cd37 --- /dev/null +++ b/.github/workflows/foxy-binary-build.yml @@ -0,0 +1,21 @@ +name: Foxy Binary Build +# Build & test all dependencies from released (binary) packages. + +on: + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '08 18 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + upstream_workspace: ros2_controllers-not-released.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build.yml b/.github/workflows/foxy-semi-binary-build.yml new file mode 100644 index 0000000000..a6fc83fbd5 --- /dev/null +++ b/.github/workflows/foxy-semi-binary-build.yml @@ -0,0 +1,21 @@ +name: Foxy Semi-Binary Build +# Build & test that compiles the main dependencies from source. + +on: + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '13 18 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + upstream_workspace: ros2_controllers.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml new file mode 100644 index 0000000000..41c663dd9c --- /dev/null +++ b/.github/workflows/galactic-abi-compatibility.yml @@ -0,0 +1,17 @@ +name: ABI Compatibility Check +on: + pull_request: + branches: + - galactic + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: galactic + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build.yml b/.github/workflows/galactic-binary-build.yml new file mode 100644 index 0000000000..eeac89e8d1 --- /dev/null +++ b/.github/workflows/galactic-binary-build.yml @@ -0,0 +1,21 @@ +name: Galactic Binary Build +# Build & test all dependencies from released (binary) packages.' + +on: + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '08 18 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: galactic + upstream_workspace: ros2_controllers-not-released.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-semi-binary-build.yml b/.github/workflows/galactic-semi-binary-build.yml new file mode 100644 index 0000000000..f7f2cd0a53 --- /dev/null +++ b/.github/workflows/galactic-semi-binary-build.yml @@ -0,0 +1,21 @@ +name: Galactic Semi-Binary Build +# Build & test that compiles the main dependencies from source. + +on: + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '13 18 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: galactic + upstream_workspace: ros2_controllers.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-source-build.yml b/.github/workflows/galactic-source-build.yml new file mode 100644 index 0000000000..d1c3aa97e8 --- /dev/null +++ b/.github/workflows/galactic-source-build.yml @@ -0,0 +1,15 @@ +name: Galactic Source Build +on: + push: + branches: + - galactic + 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: galactic + ref: galactic diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml new file mode 100644 index 0000000000..b9460bda47 --- /dev/null +++ b/.github/workflows/prerelease-check.yml @@ -0,0 +1,39 @@ +name: Pre-Release Check + +on: + workflow_dispatch: + inputs: + ros_distro: + description: 'Chose ROS distribution' + required: true + default: 'rolling' + type: choice + options: + - foxy + - galactic + - humble + - rolling + branch: + description: 'Chose branch for distro' + required: true + default: 'master' + type: choice + options: + - foxy + - galactic + - humble + - master + +jobs: + pre_release: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + with: + ref: ${{ github.event.inputs.branch }} + - name: industrial_ci + uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{ github.event.inputs.ros_distro }} + PRERELEASE: true + BASEDIR: ${{ github.workspace }}/.work diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml deleted file mode 100644 index b4a7b6f585..0000000000 --- a/.github/workflows/prerelease.yaml +++ /dev/null @@ -1,26 +0,0 @@ -# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). -# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) - -name: pre-release - -on: - workflow_dispatch: - -jobs: - default: - strategy: - fail-fast: false - matrix: - distro: [galactic, rolling] - - env: - ROS_DISTRO: ${{ matrix.distro }} - PRERELEASE: true - BASEDIR: ${{ github.workspace }}/.work - - name: "${{ matrix.distro }}" - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - name: industrial_ci - uses: ros-industrial/industrial_ci@master diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml new file mode 100644 index 0000000000..57ea222979 --- /dev/null +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -0,0 +1,106 @@ +name: Reusable industrial_ci Workflow with Cache +# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache +# author: Denis Štogl + +on: + workflow_call: + inputs: + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + + upstream_workspace: + description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' + required: true + type: string + ros_distro: + description: 'ROS_DISTRO variable for industrial_ci' + required: true + type: string + ros_repo: + description: 'ROS_REPO to run for industrial_ci. Possible values: "all", "main", "testing"' + default: 'all' + required: false + type: string + os_code_name: + description: 'OS_CODE_NAME variable for industrial_ci' + default: '' + required: false + type: string + before_install_upstream_dependencies: + description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' + default: '' + required: false + type: string + + ccache_dir: + description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.ccache' + required: false + type: string + basedir: + description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' + default: '.work' + required: false + type: string + + +jobs: + reusable_industrial_ci_with_cache: + name: ${{ inputs.os_code_name }} ${{ matrix.ROS_REPO }} ${{ inputs.ros_distro }} + strategy: + fail-fast: false + matrix: + ROS_REPO: [ main, testing ] + runs-on: ubuntu-latest + env: + CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} + BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ matrix.ROS_REPO }}-${{ github.job }} + steps: + - name: Checkout ${{ inputs.ref }} when build is not scheduled + if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name != 'schedule' }} + uses: actions/checkout@v3 + - name: Checkout ${{ inputs.ref }} on scheduled build + if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name == 'schedule' }} + uses: actions/checkout@v3 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + - name: cache target_ws + if: ${{ ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} + uses: 'ros-industrial/industrial_ci@master' + env: + UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} + ROS_DISTRO: ${{ inputs.ros_distro }} + ROS_REPO: ${{ matrix.ROS_REPO }} + OS_CODE_NAME: ${{ inputs.os_code_name }} + BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} + - name: prepare target_ws for cache + if: ${{ always() && ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws + - name: Is job skipped? + if: ${{ ! (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + run: | + echo "This job is skpped!" diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml new file mode 100644 index 0000000000..f919573a86 --- /dev/null +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -0,0 +1,53 @@ +name: Reusable industrial_ci Workflow with Cache +# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache +# author: Denis Štogl + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref: + description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' + required: true + type: string + +jobs: + reusable_ros_tooling_source_build: + name: ${{ inputs.ros_distro }} ubuntu-20.04 + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + steps: + - uses: ros-tooling/setup-ros@v0.3 + with: + required-ros-distributions: ${{ inputs.ros_distro }} + - uses: actions/checkout@v3 + with: + ref: ${{ inputs.ref }} + - uses: ros-tooling/action-ros-ci@v0.2 + with: + target-ros2-distro: ${{ inputs.ros_distro }} + # build all packages listed in the meta package + package-name: + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + gripper_controllers + position_controllers + ros2_controllers + ros2_controllers_test_nodes + velocity_controllers + vcs-repo-file-url: | + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros_distro }}/ros2.repos + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - uses: actions/upload-artifact@v1 + with: + name: colcon-logs-ubuntu-20.04 + path: ros_ws/log diff --git a/.github/workflows/rolling-abi-compatibility-last-focal.yml b/.github/workflows/rolling-abi-compatibility-last-focal.yml new file mode 100644 index 0000000000..06057e3038 --- /dev/null +++ b/.github/workflows/rolling-abi-compatibility-last-focal.yml @@ -0,0 +1,21 @@ +name: ABI Compatibility Check +on: + pull_request: + branches: + - master + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: rolling + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true + OS_CODE_NAME: focal + BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: | + export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml + rosdep update diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml new file mode 100644 index 0000000000..1b327f58fb --- /dev/null +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -0,0 +1,17 @@ +name: ABI Compatibility Check +on: + pull_request: + branches: + - master + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: rolling + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-binary-build-last-focal.yml b/.github/workflows/rolling-binary-build-last-focal.yml new file mode 100644 index 0000000000..41a8980780 --- /dev/null +++ b/.github/workflows/rolling-binary-build-last-focal.yml @@ -0,0 +1,26 @@ +name: Rolling Binary Build - Last Focal +# Build & test all dependencies from released (binary) packages. + +on: + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '08 18 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers-not-released.rolling.repos + ref_for_scheduled_build: master + ros_repo: main + os_code_name: focal + before_install_upstream_dependencies: | + export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml + rosdep update diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml new file mode 100644 index 0000000000..7e0a2e04d6 --- /dev/null +++ b/.github/workflows/rolling-binary-build.yml @@ -0,0 +1,21 @@ +name: Rolling Binary Build +# Build & test all dependencies from released (binary) packages. + +on: + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '08 18 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers-not-released.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml new file mode 100644 index 0000000000..cd35303f3f --- /dev/null +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -0,0 +1,30 @@ +name: Rolling RHEL Binary Build +on: + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + rolling_rhel_binary: + name: Rolling RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: jaronl/ros:rolling-alma + steps: + - uses: actions/checkout@v3 + 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/rolling-semi-binary-build-last-focal.yml b/.github/workflows/rolling-semi-binary-build-last-focal.yml new file mode 100644 index 0000000000..e4f6d4c91c --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-last-focal.yml @@ -0,0 +1,26 @@ +name: Rolling Semi-Binary Build - Last Focal +# Build & test that compiles the main dependencies from source. + +on: + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '13 18 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + ros_repo: main + os_code_name: focal + before_install_upstream_dependencies: | + export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml + rosdep update diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml new file mode 100644 index 0000000000..25666c0b0c --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -0,0 +1,21 @@ +name: Rolling Semi-Binary Build +# Build & test that compiles the main dependencies from source. + +on: + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '13 18 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml new file mode 100644 index 0000000000..0a0ae8de43 --- /dev/null +++ b/.github/workflows/rolling-source-build.yml @@ -0,0 +1,15 @@ +name: Rolling Source Build +on: + push: + branches: + - master + 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: rolling + ref: master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f94b77ac76..ba3dd242ac 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v4.1.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,7 +33,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.12.0 + rev: v2.31.1 hooks: - id: pyupgrade args: [--py36-plus] @@ -47,7 +47,7 @@ repos: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 3.9.0 + rev: 4.0.1 hooks: - id: flake8 args: ["--ignore=E501"] @@ -108,14 +108,14 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.9.0a1 + rev: 0.10.1 hooks: - id: doc8 args: ["--max-line-length=100", "--ignore=D001"] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.8.0 + rev: v1.9.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ @@ -125,7 +125,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.0.0 + rev: v2.1.0 hooks: - id: codespell args: ["--write-changes"] diff --git a/README.md b/README.md index ce0650020d..d7aa74a173 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,32 @@ # ros2_controllers -## CI -linux: [![Build Status](https://travis-ci.org/ros-controls/ros2_controllers.svg?branch=master)](https://travis-ci.org/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 + +ROS2 Distro | Branch | Build status | Documentation | Released packages +:---------: | :----: | :----------: | :-----------: | :---------------: +**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![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) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/)
[API Reference](https://control.ros.org/galactic/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/)
[API Reference](https://control.ros.org/foxy/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) + + +### 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: `src/$NAME$/$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: `src/$NAME$/$NAME$.repos` + +1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. -## Getting Started: -Have a look at here: https://github.com/ros-controls/ros2_control ## Acknowledgements diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos new file mode 100644 index 0000000000..56f46b6f79 --- /dev/null +++ b/ros2_controllers-not-released.foxy.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos new file mode 100644 index 0000000000..56f46b6f79 --- /dev/null +++ b/ros2_controllers-not-released.galactic.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos new file mode 100644 index 0000000000..56f46b6f79 --- /dev/null +++ b/ros2_controllers-not-released.rolling.repos @@ -0,0 +1 @@ +repositories: diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos new file mode 100644 index 0000000000..09ec6e5387 --- /dev/null +++ b/ros2_controllers.foxy.repos @@ -0,0 +1,17 @@ +repositories: + ros-controls/ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: foxy + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: foxy-devel + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: foxy-devel + angles: + type: git + url: https://github.com/ros/angles.git + version: ros2 diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos new file mode 100644 index 0000000000..2011f0c31a --- /dev/null +++ b/ros2_controllers.galactic.repos @@ -0,0 +1,17 @@ +repositories: + ros-controls/ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: galactic + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: foxy-devel + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: foxy-devel + angles: + type: git + url: https://github.com/ros/angles.git + version: ros2 diff --git a/.github/workspace.repos b/ros2_controllers.rolling.repos similarity index 100% rename from .github/workspace.repos rename to ros2_controllers.rolling.repos From 2529db8d6ceb6af7dafd16dd3225d26cd04d7655 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Fri, 25 Mar 2022 18:37:14 +0300 Subject: [PATCH 068/524] Use lifecycle node as base for controllers (#244) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add lifecycle nodes * Fix compile issues * Fix trajectory controller * pre-commit fixes * Add time and period to update function * Add lifecycle nodes * Name changes * Final fixes to get things compiled. * Fixing typo in the tests. * More changes to LifecycleNodes. Co-authored-by: Bence Magyar Co-authored-by: Denis Štogl --- .../test/test_diff_drive_controller.cpp | 18 +++++------ .../test_joint_group_effort_controller.cpp | 4 +-- .../test/test_forward_command_controller.cpp | 10 +++--- .../hardware_interface_adapter.hpp | 12 +++---- .../test/test_joint_state_broadcaster.cpp | 12 +++---- .../tolerances.hpp | 2 +- .../test/test_trajectory_actions.cpp | 2 +- .../test/test_trajectory_controller.cpp | 32 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 13 ++++---- .../test_joint_group_position_controller.cpp | 4 +-- .../test_joint_group_velocity_controller.cpp | 4 +-- 11 files changed, 57 insertions(+), 56 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ab007759ef..ce5a6b09a2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -339,11 +339,11 @@ TEST_F(TestDiffDriveController, cleanup) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); + auto state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); assignResourcesPosFeedback(); - state = controller_->activate(); + state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); waitForSetup(); @@ -358,13 +358,13 @@ TEST_F(TestDiffDriveController, cleanup) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - state = controller_->deactivate(); + state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - state = controller_->cleanup(); + state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped @@ -389,14 +389,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); + auto state = controller_->get_node()->configure(); assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); - state = controller_->activate(); + state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); // send msg @@ -415,7 +415,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) // deactivated // wait so controller process the second point when deactivated std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->deactivate(); + state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -425,12 +425,12 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; // cleanup - state = controller_->cleanup(); + state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); - state = controller_->configure(); + state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 157384423a..f8cd10bc4b 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -188,10 +188,10 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index d3dbe22c8d..e217f88a1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -250,10 +250,10 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command @@ -296,7 +296,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); auto command_msg = std::make_shared(); @@ -314,7 +314,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); - node_state = controller_->deactivate(); + node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` @@ -338,7 +338,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); // Now activate again - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // command ptr should be reset (nullptr) after activation - same check as in `update` diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index cbf38e4e0a..40fde4442c 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -26,10 +26,10 @@ // TODO(JafarAbdi): Remove experimental once the default standard is C++17 #include "experimental/optional" -#include "rclcpp/time.hpp" - #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" /** * \brief Helper class to simplify integrating the GripperActionController with @@ -46,7 +46,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional< std::reference_wrapper> /* joint_handle */, - const rclcpp::Node::SharedPtr & /* node */) + std::shared_ptr & /* node */) { return false; } @@ -73,7 +73,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional> joint_handle, - const rclcpp::Node::SharedPtr & /* node */) + const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; return true; @@ -116,7 +116,7 @@ class HardwareInterfaceAdapter public: template auto auto_declare( - const rclcpp::Node::SharedPtr & node, const std::string & name, + const std::shared_ptr & node, const std::string & name, const ParameterT & default_value) { if (!node->has_parameter(name)) @@ -132,7 +132,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional> joint_handle, - const rclcpp::Node::SharedPtr & node) + const std::shared_ptr & node) { joint_handle_ = joint_handle; // Init PID gains from ROS parameter server diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 1ab4b4323d..e124708fe5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -587,8 +587,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) { SetUpStateBroadcaster(); - auto node_state = state_broadcaster_->configure(); - node_state = state_broadcaster_->activate(); + auto node_state = state_broadcaster_->get_node()->configure(); + node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ( state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -597,10 +597,10 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) { - auto node_state = state_broadcaster_->configure(); + auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_broadcaster_->activate(); + node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); @@ -649,10 +649,10 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( const std::string & topic) { - auto node_state = state_broadcaster_->configure(); + auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_broadcaster_->activate(); + node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b2944f7622..8f3f30d476 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -92,7 +92,7 @@ struct SegmentTolerances * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - const rclcpp::Node & node, const std::vector & joint_names) + const rclcpp_lifecycle::LifecycleNode & node, const std::vector & joint_names) { const auto n_joints = joint_names.size(); SegmentTolerances tolerances; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 5bb6efaea6..a7926d2278 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -74,7 +74,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest SetUpAndActivateTrajectoryController(true, parameters); - executor_->add_node(traj_node_->get_node_base_interface()); + executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); SetUpActionClient(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 965e9a5919..8771fcee41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -70,7 +70,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) executor.add_node(traj_controller_->get_node()->get_node_base_interface()); const auto future_handle_ = std::async(std::launch::async, spin, &executor); - const auto state = traj_controller_->configure(); + const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -98,7 +98,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - traj_controller_->configure(); + traj_controller_->get_node()->configure(); ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); @@ -245,11 +245,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) traj_controller_->wait_for_trajectory(executor); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - auto state = traj_controller_->deactivate(); + auto state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - state = traj_controller_->cleanup(); + state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // update for 0.25 seconds const auto start_time = rclcpp::Clock().now(); @@ -270,7 +270,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); SetPidParameters(); - traj_controller_->configure(); + traj_controller_->get_node()->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -303,7 +303,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param std::this_thread::sleep_for(FIRST_POINT_TIME); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // deactivated - state = traj_controller_->deactivate(); + state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong? @@ -324,7 +324,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param } // cleanup - state = traj_controller_->cleanup(); + state = traj_controller_->get_node()->cleanup(); // update loop receives a new msg and updates accordingly traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -338,7 +338,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta); EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta); - state = traj_controller_->configure(); + state = traj_controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); @@ -408,7 +408,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou const int qos_level = 10; int echo_received_counter = 0; rclcpp::Subscription::SharedPtr subs = - traj_node_->create_subscription( + traj_controller_->get_node()->create_subscription( controller_name_ + "/state", qos_level, [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); @@ -721,10 +721,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) // Denis: delta was 0.1 with 0.2 works for me waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory"); + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + // Replaced trajectory is a mix of previous and current goal expected_desired.positions[0] = points_partial_new[0][0]; expected_desired.positions[1] = points_old[0][1]; @@ -758,7 +759,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) // Check that we reached end of points_old[0] trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory in the past"); + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; @@ -785,7 +786,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // Check that we reached end of points_old[0]trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory partially in the past"); + RCLCPP_INFO( + traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; @@ -810,8 +812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur subscribeToState(); rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); traj_node->set_parameter(partial_joints_parameters); - traj_controller_->configure(); - traj_controller_->activate(); + traj_controller_->get_node()->configure(); + traj_controller_->get_node()->activate(); std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; @@ -1183,7 +1185,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame auto set_parameter_and_check_result = [&]() { EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); + traj_controller_->get_node()->configure(); EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); }; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 167f82aaad..94b86a6242 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -171,24 +171,24 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); + for (const auto & param : parameters) { - traj_node_->set_parameter(param); + traj_controller_->get_node()->set_parameter(param); } if (executor) { - executor->add_node(traj_node_->get_node_base_interface()); + executor->add_node(traj_controller_->get_node()->get_node_base_interface()); } // 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_node_->set_parameter(stopped_velocity_parameters); + traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); // set pid parameters before configure SetPidParameters(); + traj_controller_->get_node()->configure(); - traj_controller_->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -237,7 +237,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->activate(); + traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } @@ -391,7 +391,6 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Publisher::SharedPtr trajectory_publisher_; std::shared_ptr traj_controller_; - std::shared_ptr traj_node_; rclcpp::Subscription::SharedPtr state_subscriber_; mutable std::mutex state_mutex_; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index be774031f0..fc93bb7dfa 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -188,10 +188,10 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index b566829f9b..1bcbaf510e 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -188,10 +188,10 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command From ee9a1063b744f6adf2ec8b690725eabad9d5ed94 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 25 Mar 2022 15:39:50 +0000 Subject: [PATCH 069/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 5 +++++ 12 files changed, 55 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e136237ebf..60774829ce 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ * use rolling mean from rcppmath (`#211 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 15b2377aa6..3ccbe822b4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b1b16f9fc1..c0a38a1045 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.0 (2022-02-23) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 6fc27346b1..9608eda538 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f3928d96f3..d00a2d0841 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6eb8f1a9b8..ce932d7894 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.0 (2022-02-23) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1c955ec1dd..95dba87d82 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ * joint_state_broadcaster to use realtime tools (`#276 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1ad1b61e2f..7aaf348006 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* JointTrajectoryController: added missing control_toolbox dependencies (`#315 `_) +* Use time argument on update function instead of node time (`#296 `_) +* Export dependency (`#310 `_) +* Contributors: DasRoteSkelett, Erick G. Islas-Osuna, Jafar Abdi, Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ * INSTANTIATE_TEST_CASE_P -> INSTANTIATE_TEST_SUITE_P (`#293 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 3d1d6dfa00..e835e2b7d7 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0f689e50db..945a5a556c 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.0 (2022-02-23) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 1a0d314b0e..14f9730d7b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.1.0 (2022-02-23) ------------------ * Move test nodes from the ros2_control_demos repository. (`#294 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b4a1247eb2..737810d7c1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + 2.1.0 (2022-02-23) ------------------ From 50a8a37392ea692aac40410bbf778a84a819945e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 25 Mar 2022 15:40:27 +0000 Subject: [PATCH 070/524] 2.2.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 60774829ce..fbbb5df21c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d660b2ae1f..2d8c76cc95 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.1.0 + 2.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3ccbe822b4..24b35e8653 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 90fd24207d..b7d11c25f7 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.1.0 + 2.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index c0a38a1045..4f94c1d87e 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ 2.1.0 (2022-02-23) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 7792a77d8a..564ae7d425 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.1.0 + 2.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 9608eda538..d606949901 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 500b156202..78e259fa9a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.1.0 + 2.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d00a2d0841..a031cf2e19 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 6939197808..cb8a1f416b 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.1.0 + 2.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ce932d7894..6389674f01 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ 2.1.0 (2022-02-23) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b7c434f88e..5b1e6251b5 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.1.0 + 2.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 95dba87d82..67c7d07de9 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e2fcb35326..208138fd19 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.1.0 + 2.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 7aaf348006..10615fb972 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * JointTrajectoryController: added missing control_toolbox dependencies (`#315 `_) * Use time argument on update function instead of node time (`#296 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index eebde38b51..e6409bd71b 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.1.0 + 2.2.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 e835e2b7d7..1c7c243ff1 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 471702a389..92b91a6955 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.1.0 + 2.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 945a5a556c..f06ebfbb86 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ 2.1.0 (2022-02-23) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d3a6985cb8..92e92b89a8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.1.0 + 2.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 14f9730d7b..d5f1f6337b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ 2.1.0 (2022-02-23) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index b3c028e4f6..2b91fb6d0e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.1.0 + 2.2.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 737810d7c1..a704ea8c01 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.0 (2022-03-25) +------------------ * Use lifecycle node as base for controllers (`#244 `_) * Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index b36eaf3d95..e60cf7b4bb 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.1.0 + 2.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From c51ba1e45ff3868f7d50b164e3ed8162db4440f1 Mon Sep 17 00:00:00 2001 From: Jafar Date: Tue, 29 Mar 2022 22:11:28 +0300 Subject: [PATCH 071/524] Remove unused include to fix JTC test (#319) --- .../test/test_load_joint_trajectory_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 6a08e62e99..7f80badebc 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -21,7 +21,6 @@ #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -#include "test_trajectory_controller_utils.hpp" TEST(TestLoadJointStateController, load_controller) { From 98eef779679a642095ce242c15e6031a95f9c56e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 Apr 2022 06:00:16 -0500 Subject: [PATCH 072/524] [JTC] Variable renaming for clearer API (#323) --- .../joint_trajectory_controller/trajectory.hpp | 2 +- joint_trajectory_controller/src/trajectory.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 36d6f0ffdd..0e2a77d30d 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -72,7 +72,7 @@ class Trajectory */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); /** diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ad77ff564..bc941c683c 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -58,11 +58,11 @@ void Trajectory::update(std::shared_ptr j } bool Trajectory::sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & expected_state, + const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) - expected_state = trajectory_msgs::msg::JointTrajectoryPoint(); + output_state = trajectory_msgs::msg::JointTrajectoryPoint(); if (trajectory_msg_->points.empty()) { @@ -98,7 +98,7 @@ bool Trajectory::sample( interpolate_between_points( t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, - expected_state); + output_state); start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); return true; @@ -118,7 +118,7 @@ bool Trajectory::sample( if (sample_time >= t0 && sample_time < t1) { - interpolate_between_points(t0, point, t1, next_point, sample_time, expected_state); + interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); return true; @@ -128,15 +128,15 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); - expected_state = (*start_segment_itr); + output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them - if (expected_state.velocities.empty()) + if (output_state.velocities.empty()) { - expected_state.velocities.resize(expected_state.positions.size(), 0.0); + output_state.velocities.resize(output_state.positions.size(), 0.0); } - if (expected_state.accelerations.empty()) + if (output_state.accelerations.empty()) { - expected_state.accelerations.resize(expected_state.positions.size(), 0.0); + output_state.accelerations.resize(output_state.positions.size(), 0.0); } return true; } From 97c94316fe9c406cf21e493cd3c58d4c1c3fd533 Mon Sep 17 00:00:00 2001 From: Akash Date: Fri, 8 Apr 2022 00:19:45 -0700 Subject: [PATCH 073/524] [JTC] Implement effort-only command interface (#225) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix trajectory tolerance parameters * Implement effort command interface for JTC * Use auto_declare for pid params * Set effort to 0 on deactivate Co-authored-by: Denis Štogl --- .../joint_trajectory_controller.hpp | 4 +- .../src/joint_trajectory_controller.cpp | 126 +++++++++++------- .../test/test_trajectory_controller.cpp | 62 ++++++++- .../test/test_trajectory_controller_utils.hpp | 11 ++ 4 files changed, 150 insertions(+), 53 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 f0e74da0c6..cf5e267800 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 @@ -141,11 +141,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_position_command_interface_ = false; bool has_velocity_command_interface_ = false; bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - // TODO(anyone): This flag is not used for now - // There should be PID-approach used as in ROS1: - // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 bool use_closed_loop_pid_adapter = false; using PidPtr = std::shared_ptr; std::vector pids_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9a43d9195e..27a599b019 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -189,39 +189,48 @@ controller_interface::return_type JointTrajectoryController::update( const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); // set values for next hardware write() + if (use_closed_loop_pid_adapter) + { + // Update PIDs + for (auto i = 0ul; i < joint_num; ++i) + { + tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired.positions[i] - state_current.positions[i], + state_desired.velocities[i] - state_current.velocities[i], + (uint64_t)period.nanoseconds()); + } + } if (has_position_command_interface_) { assign_interface_from_point(joint_command_interface_[0], state_desired.positions); } if (has_velocity_command_interface_) { - if (!use_closed_loop_pid_adapter) + if (use_closed_loop_pid_adapter) { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + assign_interface_from_point(joint_command_interface_[1], tmp_command_); } else { - // Update PIDs - for (auto i = 0ul; i < joint_num; ++i) - { - tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_desired.positions[i] - state_current.positions[i], - state_desired.velocities[i] - state_current.velocities[i], - (uint64_t)period.nanoseconds()); - } - assign_interface_from_point(joint_command_interface_[1], tmp_command_); + assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); } } if (has_acceleration_command_interface_) { assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); } - // TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter" (see ROS1) - #171 - // if (check_if_interface_type_exist( - // command_interface_types_, hardware_interface::HW_IF_EFFORT)) { - // assign_interface_from_point(joint_command_interface_[3], state_desired.effort); - // } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + } + } for (size_t index = 0; index < joint_num; ++index) { @@ -477,32 +486,14 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // 2. velocity // 2. position [velocity, [acceleration]] - // effort can't be combined with other interfaces - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) - { - if (command_interface_types_.size() == 1) - { - // TODO(anyone): This flag is not used for now - // There should be PID-approach used as in ROS1: - // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 - use_closed_loop_pid_adapter = true; - // TODO(anyone): remove the next two lines when implemented - RCLCPP_ERROR(logger, "using 'effort' command interface alone is not yet implemented yet."); - return CallbackReturn::FAILURE; - } - else - { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); - return CallbackReturn::FAILURE; - } - } - has_position_command_interface_ = contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); has_velocity_command_interface_ = contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); has_acceleration_command_interface_ = contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); if (has_velocity_command_interface_) { @@ -530,8 +521,21 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } - // TODO(livanov93): change when other option is implemented - if (has_velocity_command_interface_ && use_closed_loop_pid_adapter) + // effort can't be combined with other interfaces + if (has_effort_command_interface_) + { + if (command_interface_types_.size() == 1) + { + use_closed_loop_pid_adapter = true; + } + else + { + RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); + return CallbackReturn::FAILURE; + } + } + + if (use_closed_loop_pid_adapter) { size_t num_joints = joint_names_.size(); pids_.resize(num_joints); @@ -593,7 +597,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S if (has_velocity_state_interface_) { - if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION)) + if (!has_position_state_interface_) { RCLCPP_ERROR( logger, @@ -602,13 +606,33 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } } - else if (has_acceleration_state_interface_) + else { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; + if (has_acceleration_state_interface_) + { + RCLCPP_ERROR( + logger, + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + return CallbackReturn::FAILURE; + } + if (has_velocity_command_interface_ && command_interface_types_.size() == 1) + { + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + // effort is always used alone so no need for size check + if (has_effort_command_interface_) + { + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } } auto get_interface_list = [](const std::vector & interface_types) { @@ -808,6 +832,11 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle:: { joint_command_interface_[1][index].get().set_value(0.0); } + + if (has_effort_command_interface_) + { + joint_command_interface_[3][index].get().set_value(0.0); + } } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -845,6 +874,11 @@ bool JointTrajectoryController::reset() subscriber_is_active_ = false; joint_command_subscriber_.reset(); + for (const auto & pid : pids_) + { + pid->reset(); + } + // iterator has no default value // prev_traj_point_ptr_; traj_point_active_ptr_ = nullptr; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8771fcee41..5606dfc40c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -323,6 +323,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_LE(0.0, joint_vel_[2]); } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + // cleanup state = traj_controller_->get_node()->cleanup(); @@ -469,6 +476,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].velocities[2] = -0.1; } + if (traj_controller_->has_effort_command_interface()) + { + traj_msg.points[0].effort.resize(3); + traj_msg.points[0].effort[0] = -0.1; + traj_msg.points[0].effort[1] = -0.1; + traj_msg.points[0].effort[2] = -0.1; + } + trajectory_publisher_->publish(traj_msg); } @@ -491,6 +506,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } } /** @@ -552,6 +574,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end()) + { + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], threshold) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } // TODO(anyone): add here ckecks for acceleration commands executor.cancel(); @@ -1180,6 +1214,16 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"velocity"}), std::vector({"position", "velocity", "acceleration"})))); +// only effort controller +INSTANTIATE_TEST_CASE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); + TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { @@ -1199,10 +1243,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"bad_name"}; set_parameter_and_check_result(); - // command interfaces: effort not yet implemented - command_interface_types_ = {"effort"}; - set_parameter_and_check_result(); - // command interfaces: effort has to be only command_interface_types_ = {"effort", "position"}; set_parameter_and_check_result(); @@ -1236,4 +1276,18 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // state interfaces: acceleration without position and velocity state_interface_types_ = {"acceleration"}; set_parameter_and_check_result(); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + set_parameter_and_check_result(); + state_interface_types_ = {"velocity"}; + set_parameter_and_check_result(); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + set_parameter_and_check_result(); + state_interface_types_ = {"velocity"}; + set_parameter_and_check_result(); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 94b86a6242..d8bb8a0b47 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -36,6 +36,7 @@ const std::vector INITIAL_POS_JOINTS = { INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; +const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; } // namespace namespace test_trajectory_controllers @@ -98,6 +99,8 @@ class TestableJointTrajectoryController bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_effort_command_interface() { return has_effort_command_interface_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -117,6 +120,7 @@ class TrajectoryControllerTest : public ::testing::Test joint_state_vel_.resize(joint_names_.size(), 0.0); joint_acc_.resize(joint_names_.size(), 0.0); joint_state_acc_.resize(joint_names_.size(), 0.0); + joint_eff_.resize(joint_names_.size(), 0.0); // Default interface values - they will be overwritten by parameterized tests command_interface_types_ = {"position"}; state_interface_types_ = {"position", "velocity"}; @@ -199,6 +203,7 @@ class TrajectoryControllerTest : public ::testing::Test pos_cmd_interfaces_.reserve(joint_names_.size()); vel_cmd_interfaces_.reserve(joint_names_.size()); acc_cmd_interfaces_.reserve(joint_names_.size()); + eff_cmd_interfaces_.reserve(joint_names_.size()); pos_state_interfaces_.reserve(joint_names_.size()); vel_state_interfaces_.reserve(joint_names_.size()); acc_state_interfaces_.reserve(joint_names_.size()); @@ -210,6 +215,8 @@ class TrajectoryControllerTest : public ::testing::Test joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( joint_names_[i], hardware_interface::HW_IF_POSITION, @@ -228,6 +235,8 @@ class TrajectoryControllerTest : public ::testing::Test 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.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]; @@ -399,12 +408,14 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_pos_; std::vector joint_vel_; std::vector joint_acc_; + std::vector joint_eff_; std::vector joint_state_pos_; std::vector joint_state_vel_; std::vector joint_state_acc_; std::vector pos_cmd_interfaces_; std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; + std::vector eff_cmd_interfaces_; std::vector pos_state_interfaces_; std::vector vel_state_interfaces_; std::vector acc_state_interfaces_; From 76bf6ae70c432565c8767fba14c9adc75028292e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 14 Apr 2022 18:17:23 +0200 Subject: [PATCH 074/524] Correct name of a workflow (#330) --- .github/workflows/rolling-abi-compatibility-last-focal.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling-abi-compatibility-last-focal.yml b/.github/workflows/rolling-abi-compatibility-last-focal.yml index 06057e3038..9ad07243d9 100644 --- a/.github/workflows/rolling-abi-compatibility-last-focal.yml +++ b/.github/workflows/rolling-abi-compatibility-last-focal.yml @@ -1,4 +1,4 @@ -name: ABI Compatibility Check +name: ABI Compatibility Check - last Focal on: pull_request: branches: From 649bdbe25bca5eb2da36379230799feb6af689e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 20 Apr 2022 18:13:38 +0200 Subject: [PATCH 075/524] Use CallbackReturn from controller_interface namespace (#333) Co-authored-by: Bence Magyar --- .../diff_drive_controller.hpp | 24 ++++--- .../src/diff_drive_controller.cpp | 63 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 2 +- .../joint_group_effort_controller.hpp | 7 +-- .../src/joint_group_effort_controller.cpp | 10 +-- .../test_joint_group_effort_controller.cpp | 2 +- .../force_torque_sensor_broadcaster.hpp | 13 ++-- .../src/force_torque_sensor_broadcaster.cpp | 26 ++++---- .../test_force_torque_sensor_broadcaster.cpp | 6 +- .../forward_command_controller.hpp | 12 ++-- .../src/forward_command_controller.cpp | 24 +++---- .../test/test_forward_command_controller.cpp | 2 +- .../gripper_action_controller.hpp | 13 ++-- .../gripper_action_controller_impl.hpp | 34 +++++----- .../imu_sensor_broadcaster.hpp | 13 ++-- .../src/imu_sensor_broadcaster.cpp | 9 +-- .../test/test_imu_sensor_broadcaster.cpp | 6 +- .../joint_state_broadcaster.hpp | 13 ++-- .../src/joint_state_broadcaster.cpp | 8 +-- .../test/test_joint_state_broadcaster.cpp | 6 +- .../joint_trajectory_controller.hpp | 22 ++++--- .../src/joint_trajectory_controller.cpp | 20 +++--- .../test/test_trajectory_controller_utils.hpp | 3 +- .../joint_group_position_controller.hpp | 4 +- .../src/joint_group_position_controller.cpp | 2 +- .../test_joint_group_position_controller.cpp | 2 +- .../joint_group_velocity_controller.hpp | 6 +- .../src/joint_group_velocity_controller.cpp | 4 +- .../test_joint_group_velocity_controller.cpp | 2 +- 29 files changed, 188 insertions(+), 170 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 12a9bb75e0..ce357e4116 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 @@ -44,8 +44,6 @@ namespace diff_drive_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class DiffDriveController : public controller_interface::ControllerInterface { using Twist = geometry_msgs::msg::TwistStamped; @@ -65,25 +63,31 @@ class DiffDriveController : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; DIFF_DRIVE_CONTROLLER_PUBLIC - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; protected: struct WheelHandle @@ -93,7 +97,7 @@ class DiffDriveController : public controller_interface::ControllerInterface }; const char * feedback_type() const; - CallbackReturn configure_side( + controller_interface::CallbackReturn configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 382740d8f4..cee17d01f1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -53,7 +53,7 @@ const char * DiffDriveController::feedback_type() const return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; } -CallbackReturn DiffDriveController::on_init() +controller_interface::CallbackReturn DiffDriveController::on_init() { try { @@ -105,10 +105,10 @@ CallbackReturn DiffDriveController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } InterfaceConfiguration DiffDriveController::command_interface_configuration() const @@ -296,7 +296,8 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } -CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_configure( + const rclcpp_lifecycle::State &) { auto logger = node_->get_logger(); @@ -309,13 +310,13 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & RCLCPP_ERROR( logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", left_wheel_names_.size(), right_wheel_names_.size()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (left_wheel_names_.empty()) { RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } wheel_params_.separation = node_->get_parameter("wheel_separation").as_double(); @@ -397,7 +398,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & if (!reset()) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // left and right sides are both equal at this point @@ -503,59 +504,64 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; previous_update_timestamp_ = node_->get_clock()->now(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_activate( + const rclcpp_lifecycle::State &) { const auto left_result = configure_side("left", left_wheel_names_, registered_left_wheel_handles_); const auto right_result = configure_side("right", right_wheel_names_, registered_right_wheel_handles_); - if (left_result == CallbackReturn::ERROR || right_result == CallbackReturn::ERROR) + if ( + left_result == controller_interface::CallbackReturn::ERROR || + right_result == controller_interface::CallbackReturn::ERROR) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } is_halted = false; subscriber_is_active_ = true; RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DiffDriveController::on_deactivate(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_deactivate( + const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DiffDriveController::on_cleanup(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_cleanup( + const rclcpp_lifecycle::State &) { if (!reset()) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } received_velocity_msg_ptr_.set(std::make_shared()); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DiffDriveController::on_error(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_error(const rclcpp_lifecycle::State &) { if (!reset()) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } bool DiffDriveController::reset() @@ -578,9 +584,10 @@ bool DiffDriveController::reset() return true; } -CallbackReturn DiffDriveController::on_shutdown(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn DiffDriveController::on_shutdown( + const rclcpp_lifecycle::State &) { - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } void DiffDriveController::halt() @@ -596,7 +603,7 @@ void DiffDriveController::halt() halt_wheels(registered_right_wheel_handles_); } -CallbackReturn DiffDriveController::configure_side( +controller_interface::CallbackReturn DiffDriveController::configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles) { @@ -605,7 +612,7 @@ CallbackReturn DiffDriveController::configure_side( if (wheel_names.empty()) { RCLCPP_ERROR(logger, "No '%s' wheel names specified", side.c_str()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // register handles @@ -623,7 +630,7 @@ CallbackReturn DiffDriveController::configure_side( if (state_handle == state_interfaces_.cend()) { RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } const auto command_handle = std::find_if( @@ -636,14 +643,14 @@ CallbackReturn DiffDriveController::configure_side( if (command_handle == command_interfaces_.end()) { RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } registered_handles.emplace_back( WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } } // namespace diff_drive_controller diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ce5a6b09a2..236f34e9a2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -28,7 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::LoanedCommandInterface; diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index a7228a2cae..562bbea52f 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -23,8 +23,6 @@ namespace effort_controllers { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * \brief Forward command controller for a set of effort controlled joints (linear or angular). * @@ -42,10 +40,11 @@ class JointGroupEffortController : public forward_command_controller::ForwardCom JointGroupEffortController(); EFFORT_CONTROLLERS_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; EFFORT_CONTROLLERS_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; }; } // namespace effort_controllers diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index e607e3c27f..b049292fe0 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -29,10 +29,10 @@ JointGroupEffortController::JointGroupEffortController() interface_name_ = hardware_interface::HW_IF_EFFORT; } -CallbackReturn JointGroupEffortController::on_init() +controller_interface::CallbackReturn JointGroupEffortController::on_init() { auto ret = forward_command_controller::ForwardCommandController::on_init(); - if (ret != CallbackReturn::SUCCESS) + if (ret != controller_interface::CallbackReturn::SUCCESS) { return ret; } @@ -47,13 +47,13 @@ CallbackReturn JointGroupEffortController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn JointGroupEffortController::on_deactivate( +controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { auto ret = ForwardCommandController::on_deactivate(previous_state); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index f8cd10bc4b..256a4ce465 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index a1c0be6335..023c59cae0 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -33,8 +33,6 @@ namespace force_torque_sensor_broadcaster { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface { public: @@ -47,16 +45,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_init() override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update( diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 5f5c880c39..e79ec4b007 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -28,7 +28,7 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() { } -CallbackReturn ForceTorqueSensorBroadcaster::on_init() +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() { try { @@ -44,13 +44,13 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ForceTorqueSensorBroadcaster::on_configure( +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { sensor_name_ = node_->get_parameter("sensor_name").as_string(); @@ -70,7 +70,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( node_->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (!sensor_name_.empty() && !no_interface_names_defined) @@ -79,14 +79,14 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( node_->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } frame_id_ = node_->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (!sensor_name_.empty()) @@ -114,7 +114,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } realtime_publisher_->lock(); @@ -122,7 +122,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_publisher_->unlock(); RCLCPP_DEBUG(node_->get_logger(), "configure successful"); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration @@ -142,18 +142,18 @@ ForceTorqueSensorBroadcaster::state_interface_configuration() const return state_interfaces_config; } -CallbackReturn ForceTorqueSensorBroadcaster::on_activate( +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { force_torque_sensor_->release_interfaces(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type ForceTorqueSensorBroadcaster::update( 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 db8f6e7022..74c636c596 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 @@ -35,10 +35,8 @@ using hardware_interface::LoanedStateInterface; namespace { -constexpr auto NODE_SUCCESS = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) { diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 9926a7e9b7..5a1da3d40e 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -30,7 +30,6 @@ namespace forward_command_controller { using CmdType = std_msgs::msg::Float64MultiArray; -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** * \brief Forward command controller for a set of joints. @@ -57,16 +56,19 @@ class ForwardCommandController : public controller_interface::ControllerInterfac controller_interface::InterfaceConfiguration state_interface_configuration() const override; FORWARD_COMMAND_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; FORWARD_COMMAND_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::return_type update( diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 33fa9bbd34..fc9564b9be 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -37,7 +37,7 @@ ForwardCommandController::ForwardCommandController() { } -CallbackReturn ForwardCommandController::on_init() +controller_interface::CallbackReturn ForwardCommandController::on_init() { try { @@ -48,13 +48,13 @@ CallbackReturn ForwardCommandController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ForwardCommandController::on_configure( +controller_interface::CallbackReturn ForwardCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_ = node_->get_parameter("joints").as_string_array(); @@ -62,7 +62,7 @@ CallbackReturn ForwardCommandController::on_configure( if (joint_names_.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // Specialized, child controllers set interfaces before calling configure function. @@ -74,7 +74,7 @@ CallbackReturn ForwardCommandController::on_configure( if (interface_name_.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } joints_command_subscriber_ = get_node()->create_subscription( @@ -82,7 +82,7 @@ CallbackReturn ForwardCommandController::on_configure( [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration @@ -106,7 +106,7 @@ ForwardCommandController::state_interface_configuration() const controller_interface::interface_configuration_type::NONE}; } -CallbackReturn ForwardCommandController::on_activate( +controller_interface::CallbackReturn ForwardCommandController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // check if we have all resources defined in the "points" parameter @@ -120,21 +120,21 @@ CallbackReturn ForwardCommandController::on_activate( RCLCPP_ERROR( node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), ordered_interfaces.size()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_.reset(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ForwardCommandController::on_deactivate( +controller_interface::CallbackReturn ForwardCommandController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer rt_command_ptr_.reset(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type ForwardCommandController::update( diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index e217f88a1c..4011f13d71 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -33,7 +33,7 @@ #include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -using CallbackReturn = forward_command_controller::ForwardCommandController::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index c041fb2de1..9f1b74d3ab 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -47,8 +47,6 @@ namespace gripper_action_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * \brief Controller for executing a gripper command action for simple * single-dof grippers. @@ -92,16 +90,19 @@ class GripperActionController : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; GRIPPER_ACTION_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; GRIPPER_ACTION_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; // pre-allocated memory that is re-used to set the realtime buffer diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 9598d03bb1..42aee83b4c 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -24,8 +24,6 @@ namespace gripper_action_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - template void GripperActionController::preempt_active_goal() { @@ -39,7 +37,7 @@ void GripperActionController::preempt_active_goal() } template -CallbackReturn GripperActionController::on_init() +controller_interface::CallbackReturn GripperActionController::on_init() { try { @@ -54,10 +52,10 @@ CallbackReturn GripperActionController::on_init() catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } template @@ -188,7 +186,7 @@ void GripperActionController::check_for_success( } template -CallbackReturn GripperActionController::on_configure( +controller_interface::CallbackReturn GripperActionController::on_configure( const rclcpp_lifecycle::State &) { const auto logger = node_->get_logger(); @@ -205,7 +203,7 @@ CallbackReturn GripperActionController::on_configure( if (joint_name_.empty()) { RCLCPP_ERROR(logger, "Could not find joint name on param server"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // Default tolerances @@ -218,10 +216,10 @@ CallbackReturn GripperActionController::on_configure( stall_velocity_threshold_ = node_->get_parameter("stall_velocity_threshold").as_double(); stall_timeout_ = node_->get_parameter("stall_timeout").as_double(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } template -CallbackReturn GripperActionController::on_activate( +controller_interface::CallbackReturn GripperActionController::on_activate( const rclcpp_lifecycle::State &) { auto position_command_interface_it = std::find_if( @@ -232,7 +230,7 @@ CallbackReturn GripperActionController::on_activate( if (position_command_interface_it == command_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position command interface"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (position_command_interface_it->get_name() != joint_name_) { @@ -240,7 +238,7 @@ CallbackReturn GripperActionController::on_activate( node_->get_logger(), "Position command interface is different than joint name `" << position_command_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), @@ -250,7 +248,7 @@ CallbackReturn GripperActionController::on_activate( if (position_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 position state interface"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (position_state_interface_it->get_name() != joint_name_) { @@ -258,7 +256,7 @@ CallbackReturn GripperActionController::on_activate( node_->get_logger(), "Position state interface is different than joint name `" << position_state_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), @@ -268,7 +266,7 @@ CallbackReturn GripperActionController::on_activate( if (velocity_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(node_->get_logger(), "Expected 1 velocity state interface"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } if (velocity_state_interface_it->get_name() != joint_name_) { @@ -276,7 +274,7 @@ CallbackReturn GripperActionController::on_activate( node_->get_logger(), "Velocity command interface is different than joint name `" << velocity_state_interface_it->get_name() << "` != `" << joint_name_ << "`"); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } joint_position_command_interface_ = *position_command_interface_it; @@ -304,18 +302,18 @@ CallbackReturn GripperActionController::on_activate( std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } template -CallbackReturn GripperActionController::on_deactivate( +controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { joint_position_command_interface_ = std::experimental::nullopt; joint_position_state_interface_ = std::experimental::nullopt; joint_velocity_state_interface_ = std::experimental::nullopt; release_interfaces(); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } template diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 9a736d5cfb..11310b5b70 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -33,8 +33,6 @@ namespace imu_sensor_broadcaster { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class IMUSensorBroadcaster : public controller_interface::ControllerInterface { public: @@ -44,16 +42,19 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - IMU_SENSOR_BROADCASTER_PUBLIC CallbackReturn on_init() override; + IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; IMU_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; IMU_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; IMU_SENSOR_BROADCASTER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update( diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 23aa33777c..4e88960995 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -23,7 +23,7 @@ namespace imu_sensor_broadcaster { -CallbackReturn IMUSensorBroadcaster::on_init() +controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() { try { @@ -40,7 +40,7 @@ CallbackReturn IMUSensorBroadcaster::on_init() return CallbackReturn::SUCCESS; } -CallbackReturn IMUSensorBroadcaster::on_configure( +controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { sensor_name_ = node_->get_parameter("sensor_name").as_string(); @@ -99,13 +99,14 @@ controller_interface::InterfaceConfiguration IMUSensorBroadcaster::state_interfa return state_interfaces_config; } -CallbackReturn IMUSensorBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn IMUSensorBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { imu_sensor_->assign_loaned_state_interfaces(state_interfaces_); return CallbackReturn::SUCCESS; } -CallbackReturn IMUSensorBroadcaster::on_deactivate( +controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { imu_sensor_->release_interfaces(); diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index d27be76b3b..1ddc6779ce 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -35,10 +35,8 @@ using hardware_interface::LoanedStateInterface; namespace { -constexpr auto NODE_SUCCESS = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) { diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 568840ae7a..f3716149a5 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -30,8 +30,6 @@ namespace joint_state_broadcaster { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * \brief Joint State Broadcaster for all or some state in a ros2_control system. * @@ -73,16 +71,19 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_STATE_BROADCASTER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; JOINT_STATE_BROADCASTER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_STATE_BROADCASTER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; protected: bool init_joint_data(); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 272a390152..83656f10f6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -46,7 +46,7 @@ using hardware_interface::HW_IF_VELOCITY; JointStateBroadcaster::JointStateBroadcaster() {} -CallbackReturn JointStateBroadcaster::on_init() +controller_interface::CallbackReturn JointStateBroadcaster::on_init() { try { @@ -100,7 +100,7 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf return state_interfaces_config; } -CallbackReturn JointStateBroadcaster::on_configure( +controller_interface::CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); @@ -177,7 +177,7 @@ CallbackReturn JointStateBroadcaster::on_configure( return CallbackReturn::SUCCESS; } -CallbackReturn JointStateBroadcaster::on_activate( +controller_interface::CallbackReturn JointStateBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { if (!init_joint_data()) @@ -203,7 +203,7 @@ CallbackReturn JointStateBroadcaster::on_activate( return CallbackReturn::SUCCESS; } -CallbackReturn JointStateBroadcaster::on_deactivate( +controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index e124708fe5..f4fe654f5b 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -43,10 +43,8 @@ using testing::SizeIs; namespace { -constexpr auto NODE_SUCCESS = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) { 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 cf5e267800..c0dafeba71 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 @@ -57,8 +57,6 @@ class State; namespace joint_trajectory_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface @@ -86,25 +84,31 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; protected: std::vector joint_names_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 27a599b019..37a6d5accd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -50,7 +50,7 @@ JointTrajectoryController::JointTrajectoryController() { } -CallbackReturn JointTrajectoryController::on_init() +controller_interface::CallbackReturn JointTrajectoryController::on_init() { try { @@ -438,7 +438,8 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } -CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) { const auto logger = node_->get_logger(); @@ -746,7 +747,8 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::SUCCESS; } -CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_activate( + const rclcpp_lifecycle::State &) { // order all joints in the storage for (const auto & interface : command_interface_types_) @@ -817,7 +819,8 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St return CallbackReturn::SUCCESS; } -CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State &) { // TODO(anyone): How to halt when using effort commands? for (size_t index = 0; index < joint_names_.size(); ++index) @@ -851,7 +854,8 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle:: return CallbackReturn::SUCCESS; } -CallbackReturn JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( + const rclcpp_lifecycle::State &) { // go home traj_home_point_ptr_->update(traj_msg_home_ptr_); @@ -860,7 +864,8 @@ CallbackReturn JointTrajectoryController::on_cleanup(const rclcpp_lifecycle::Sta return CallbackReturn::SUCCESS; } -CallbackReturn JointTrajectoryController::on_error(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_error( + const rclcpp_lifecycle::State &) { if (!reset()) { @@ -895,7 +900,8 @@ bool JointTrajectoryController::reset() return true; } -CallbackReturn JointTrajectoryController::on_shutdown(const rclcpp_lifecycle::State &) +controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( + const rclcpp_lifecycle::State &) { // TODO(karsten1987): what to do? diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index d8bb8a0b47..c4becd25d6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -48,7 +48,8 @@ class TestableJointTrajectoryController using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController; using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); // this class can still be useful without the wait set diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 0b3cb0796b..47705b6a3d 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -23,8 +23,6 @@ namespace position_controllers { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * \brief Forward command controller for a set of position controlled joints (linear or angular). * @@ -41,7 +39,7 @@ class JointGroupPositionController : public forward_command_controller::ForwardC POSITION_CONTROLLERS_PUBLIC JointGroupPositionController(); - POSITION_CONTROLLERS_PUBLIC CallbackReturn on_init() override; + POSITION_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; }; } // namespace position_controllers diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index e52f9425fd..10829dc9c7 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -29,7 +29,7 @@ JointGroupPositionController::JointGroupPositionController() interface_name_ = hardware_interface::HW_IF_POSITION; } -CallbackReturn JointGroupPositionController::on_init() +controller_interface::CallbackReturn JointGroupPositionController::on_init() { auto ret = forward_command_controller::ForwardCommandController::on_init(); if (ret != CallbackReturn::SUCCESS) diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index fc93bb7dfa..a712cc81c2 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index 2c45448cf2..b5c36f433a 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -23,7 +23,6 @@ namespace velocity_controllers { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** * \brief Forward command controller for a set of velocity controlled joints (linear or angular). * @@ -40,10 +39,11 @@ class JointGroupVelocityController : public forward_command_controller::ForwardC VELOCITY_CONTROLLERS_PUBLIC JointGroupVelocityController(); - VELOCITY_CONTROLLERS_PUBLIC CallbackReturn on_init() override; + VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; VELOCITY_CONTROLLERS_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; }; } // namespace velocity_controllers diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index c2c7d762bd..f1a61d4fdd 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -29,7 +29,7 @@ JointGroupVelocityController::JointGroupVelocityController() interface_name_ = hardware_interface::HW_IF_VELOCITY; } -CallbackReturn JointGroupVelocityController::on_init() +controller_interface::CallbackReturn JointGroupVelocityController::on_init() { auto ret = ForwardCommandController::on_init(); if (ret != CallbackReturn::SUCCESS) @@ -53,7 +53,7 @@ CallbackReturn JointGroupVelocityController::on_init() return CallbackReturn::SUCCESS; } -CallbackReturn JointGroupVelocityController::on_deactivate( +controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { auto ret = ForwardCommandController::on_deactivate(previous_state); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 1bcbaf510e..43c1545461 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -27,7 +27,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace From 0e7c2606d354ee96978cc6f33c7ebda891b22273 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 20 Apr 2022 23:41:59 +0200 Subject: [PATCH 076/524] [JTC] Allow integration of states in goal trajectories (#190) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add allow_integration flag * Added position and velocity deduction to trajectory. * Added support for deduction of states from their derivatives. * Apply suggestions from code review Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com> * Correct warnings and formating. * Update test to use more human-readable values. * Apply reviewers comments. * Last test fixes. Co-authored-by: Denis Štogl Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com> --- .../joint_trajectory_controller.hpp | 2 + .../trajectory.hpp | 38 +- .../src/joint_trajectory_controller.cpp | 28 +- .../src/trajectory.cpp | 67 +++- .../test/test_trajectory.cpp | 358 ++++++++++++++++++ .../test/test_trajectory_controller.cpp | 69 +++- 6 files changed, 533 insertions(+), 29 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 c0dafeba71..1f5a75b4a4 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 @@ -129,6 +129,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// This is useful when robot is not exactly following the commanded trajectory. bool open_loop_control_ = false; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + /// Allow integration in goal trajectories to accept goals without position or velocity specified + bool allow_integration_in_goal_trajectories_ = false; // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 0e2a77d30d..7a4b495b70 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -58,18 +58,27 @@ class Trajectory /// Find the segment (made up of 2 points) and its expected state from the /// containing trajectory. /** - * Specific case returns for start_segment_itr and end_segment_itr: - * - Sampling before the trajectory start: - * start_segment_itr = begin(), end_segment_itr = begin() - * - Sampling exactly on a point of the trajectory: - * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr - * - Sampling between points: - * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr - * - Sampling after entire trajectory: - * start_segment_itr = --end(), end_segment_itr = end() - * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() - * return false - */ + * Sampling trajectory at given \p sample_time. + * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively. + * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment. + * + * Specific case returns for start_segment_itr and end_segment_itr: + * - Sampling before the trajectory start: + * start_segment_itr = begin(), end_segment_itr = begin() + * - Sampling exactly on a point of the trajectory: + * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr + * - Sampling between points: + * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr + * - Sampling after entire trajectory: + * start_segment_itr = --end(), end_segment_itr = end() + * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() + * return false + * + * \param[in] sample_time Time at which trajectory will be sampled. + * \param[out] expected_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state, @@ -125,6 +134,11 @@ class Trajectory bool is_sampled_already() const { return sampled_already_; } private: + void deduce_from_derivatives( + trajectory_msgs::msg::JointTrajectoryPoint & first_state, + trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, + const double delta_t); + std::shared_ptr trajectory_msg_; rclcpp::Time trajectory_start_time_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 37a6d5accd..0af506b83e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -62,6 +62,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() auto_declare("action_monitor_rate", 20.0); auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); auto_declare("open_loop_control", open_loop_control_); + auto_declare( + "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); auto_declare("constraints.stopped_velocity_tolerance", 0.01); auto_declare("constraints.goal_time", 0.0); } @@ -138,6 +140,7 @@ controller_interface::return_type JointTrajectoryController::update( { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); + // TODO(denis): Add here integration of position and velocity traj_external_point_ptr_->update(*new_external_msg); } @@ -178,7 +181,6 @@ controller_interface::return_type JointTrajectoryController::update( // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - // TODO(anyone): this is kind-of open-loop concept? I am right? const bool valid_point = (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr); @@ -659,6 +661,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Read parameters customizing controller for special cases open_loop_control_ = node_->get_parameter("open_loop_control").get_value(); + allow_integration_in_goal_trajectories_ = + node_->get_parameter("allow_integration_in_goal_trajectories").get_value(); // subscriber callback // non realtime @@ -1205,7 +1209,27 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; - if ( + // This currently supports only position, velocity and acceleration inputs + if (allow_integration_in_goal_trajectories_) + { + const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty(); + const bool position_error = + !points[i].positions.empty() && + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); + const bool velocity_error = + !points[i].velocities.empty() && + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); + const bool acceleration_error = + !points[i].accelerations.empty() && + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, false); + if (all_empty || position_error || velocity_error || acceleration_error) + { + return false; + } + } + else if ( !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || !validate_trajectory_point_field( diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index bc941c683c..281302e5f4 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -89,16 +89,21 @@ bool Trajectory::sample( } // current time hasn't reached traj time of the first point in the msg yet - const auto & first_point_in_msg = trajectory_msg_->points[0]; - const rclcpp::Duration offset = first_point_in_msg.time_from_start; - const rclcpp::Time first_point_timestamp = trajectory_start_time_ + offset; + auto & first_point_in_msg = trajectory_msg_->points[0]; + const rclcpp::Time first_point_timestamp = + trajectory_start_time_ + first_point_in_msg.time_from_start; + if (sample_time < first_point_timestamp) { - const rclcpp::Time t0 = time_before_traj_msg_; + // it changes points only if position and velocity are not exist, but their derivatives + deduce_from_derivatives( + state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), + (first_point_timestamp - time_before_traj_msg_).seconds()); interpolate_between_points( - t0, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, sample_time, - output_state); + time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, + sample_time, output_state); + start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); return true; @@ -108,17 +113,20 @@ bool Trajectory::sample( const auto last_idx = trajectory_msg_->points.size() - 1; for (size_t i = 0; i < last_idx; ++i) { - const auto & point = trajectory_msg_->points[i]; - const auto & next_point = trajectory_msg_->points[i + 1]; + auto & point = trajectory_msg_->points[i]; + auto & next_point = trajectory_msg_->points[i + 1]; - const rclcpp::Duration t0_offset = point.time_from_start; - const rclcpp::Duration t1_offset = next_point.time_from_start; - const rclcpp::Time t0 = trajectory_start_time_ + t0_offset; - const rclcpp::Time t1 = trajectory_start_time_ + t1_offset; + const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start; + const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start; if (sample_time >= t0 && sample_time < t1) { + // it changes points only if position and velocity are not exist, but their derivatives + deduce_from_derivatives( + point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); + interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); + start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); return true; @@ -272,6 +280,41 @@ void Trajectory::interpolate_between_points( } } +void Trajectory::deduce_from_derivatives( + trajectory_msgs::msg::JointTrajectoryPoint & first_state, + trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t) +{ + if (second_state.positions.empty()) + { + second_state.positions.resize(dim); + if (first_state.velocities.empty()) + { + first_state.velocities.resize(dim, 0.0); + } + if (second_state.velocities.empty()) + { + second_state.velocities.resize(dim); + if (first_state.accelerations.empty()) + { + first_state.accelerations.resize(dim, 0.0); + } + for (size_t i = 0; i < dim; ++i) + { + second_state.velocities[i] = + first_state.velocities[i] + + (first_state.accelerations[i] + second_state.accelerations[i]) * 0.5 * delta_t; + } + } + for (size_t i = 0; i < dim; ++i) + { + // second state velocity should be reached on the end of the segment, so use middle + second_state.positions[i] = + first_state.positions[i] + + (first_state.velocities[i] + second_state.velocities[i]) * 0.5 * delta_t; + } + } +} + TrajectoryPointConstIter Trajectory::begin() const { THROW_ON_NULLPTR(trajectory_msg_) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index ff820be446..3ad0388acd 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -280,3 +280,361 @@ TEST(TestTrajectory, interpolation_pos_vel_accel) EXPECT_NEAR(end_state.accelerations[0], expected_state.accelerations[0], EPS); } } + +TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double velocity_first_seg = 1.0; + double velocity_second_seg = 2.0; + double velocity_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.velocities.push_back(velocity_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.velocities.push_back(velocity_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.velocities.push_back(velocity_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + point_before_msg.velocities.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(point_before_msg.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR((velocity_first_seg / time_first_seg), expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = + traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + EXPECT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + double half_current_to_p1 = + point_before_msg.positions[0] + + (point_before_msg.velocities[0] + + ((point_before_msg.velocities[0] + p1.velocities[0]) / 2 - point_before_msg.velocities[0]) / + 2) * + 0.5; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0] / 2, expected_state.velocities[0], EPS); + EXPECT_NEAR((velocity_first_seg / time_first_seg), expected_state.accelerations[0], EPS); + } + + // sample 1s after msg + double position_first_seg = + point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_second_seg - velocity_first_seg / (time_second_seg - time_first_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 1.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + double half_p1_to_p2 = + position_first_seg + + (p1.velocities[0] + ((p1.velocities[0] + p2.velocities[0]) / 2 - p1.velocities[0]) / 2) * 0.5; + EXPECT_NEAR(half_p1_to_p2, expected_state.positions[0], EPS); + double half_p1_to_p2_vel = (p1.velocities[0] + p2.velocities[0]) / 2; + EXPECT_NEAR(half_p1_to_p2_vel, expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_second_seg - velocity_first_seg / (time_second_seg - time_first_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 2s after msg + double position_second_seg = position_first_seg + (p1.velocities[0] + p2.velocities[0]) / 2 * + (time_second_seg - time_first_seg); + { + traj.sample(time_now + rclcpp::Duration::from_seconds(2), expected_state, start, end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p2.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_third_seg - velocity_second_seg / (time_third_seg - time_second_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 2.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + double half_p2_to_p3 = + position_second_seg + + (p2.velocities[0] + ((p2.velocities[0] + p3.velocities[0]) / 2 - p2.velocities[0]) / 2) * 0.5; + EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); + double half_p2_to_p3_vel = (p2.velocities[0] + p3.velocities[0]) / 2; + EXPECT_NEAR(half_p2_to_p3_vel, expected_state.velocities[0], EPS); + EXPECT_NEAR( + (velocity_third_seg - velocity_second_seg / (time_third_seg - time_second_seg)), + expected_state.accelerations[0], EPS); + } + + // sample 3s after msg + double position_third_seg = position_second_seg + (p2.velocities[0] + p3.velocities[0]) / 2 * + (time_third_seg - time_second_seg); + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); + // the goal is reached so no acceleration anymore + EXPECT_NEAR(0, expected_state.accelerations[0], EPS); + } + + // sample past given points - movement virtually stops + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } +} + +// This test is added because previous one behaved strange if +// "point_before_msg.velocities.push_back(0.0);" was not defined +TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_without_vel) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double velocity_first_seg = 1.0; + double velocity_second_seg = 2.0; + double velocity_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.velocities.push_back(velocity_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.velocities.push_back(velocity_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.velocities.push_back(velocity_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(0.0, expected_state.velocities[0], EPS); + // is 0 because point_before_msg does not have velocity defined + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = + traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + EXPECT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + // double half_current_to_p1 = point_before_msg.positions[0] + + // (point_before_msg.velocities[0] + + // ((point_before_msg.velocities[0] + p1.velocities[0]) / 2 - + // point_before_msg.velocities[0]) / 2) * 0.5; + double half_current_to_p1 = + point_before_msg.positions[0] + (0.0 + ((0.0 + p1.velocities[0]) / 2 - 0.0) / 2) * 0.5; + EXPECT_NEAR(half_current_to_p1, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0] / 2, expected_state.velocities[0], EPS); + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } + + // sample 1s after msg + double position_first_seg = + point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(p1.velocities[0], expected_state.velocities[0], EPS); + EXPECT_NEAR(1.0, expected_state.accelerations[0], EPS); + } +} + +TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) +{ + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + // definitions + double time_first_seg = 1.0; + double time_second_seg = 2.0; + double time_third_seg = 3.0; + double acceleration_first_seg = 1.0; + double acceleration_second_seg = 2.0; + double acceleration_third_seg = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.accelerations.push_back(acceleration_first_seg); + p1.time_from_start = rclcpp::Duration::from_seconds(time_first_seg); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.accelerations.push_back(acceleration_second_seg); + p2.time_from_start = rclcpp::Duration::from_seconds(time_second_seg); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.accelerations.push_back(acceleration_third_seg); + p3.time_from_start = rclcpp::Duration::from_seconds(time_third_seg); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + point_before_msg.velocities.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR(0.0, expected_state.velocities[0], EPS); + // is 0 because point_before_msg does not have velocity defined + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // sample before time_now + { + bool result = + traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + EXPECT_EQ(result, false); + } + + // Sample only on points testing of intermediate values is too complex and not necessary + + // sample 1s after msg + double velocity_first_seg = + point_before_msg.velocities[0] + (0.0 + p1.accelerations[0]) / 2 * time_first_seg; + double position_first_seg = + point_before_msg.positions[0] + (0.0 + velocity_first_seg) / 2 * time_first_seg; + { + traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ((++traj.begin()), end); + EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_first_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p1.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample 2s after msg + double velocity_second_seg = velocity_first_seg + (p1.accelerations[0] + p2.accelerations[0]) / + 2 * (time_second_seg - time_first_seg); + double position_second_seg = position_first_seg + (velocity_first_seg + velocity_second_seg) / 2 * + (time_second_seg - time_first_seg); + { + traj.sample(time_now + rclcpp::Duration::from_seconds(2), expected_state, start, end); + EXPECT_EQ((++traj.begin()), start); + EXPECT_EQ((--traj.end()), end); + EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_second_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p2.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample 3s after msg + double velocity_third_seg = velocity_second_seg + (p2.accelerations[0] + p3.accelerations[0]) / + 2 * (time_third_seg - time_second_seg); + double position_third_seg = position_second_seg + (velocity_second_seg + velocity_third_seg) / 2 * + (time_third_seg - time_second_seg); + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_third_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p3.accelerations[0], expected_state.accelerations[0], EPS); + } + + // sample past given points - movement virtually stops + { + traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + EXPECT_EQ((--traj.end()), start); + EXPECT_EQ(traj.end(), end); + EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(velocity_third_seg, expected_state.velocities[0], EPS); + EXPECT_NEAR(p3.accelerations[0], expected_state.accelerations[0], EPS); + } +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 5606dfc40c..7e6b6b8521 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -269,8 +269,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); - SetPidParameters(); - traj_controller_->get_node()->configure(); + SetParameters(); // This call is replacing the way parameters are set via launch + traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -670,8 +670,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe TEST_P(TrajectoryControllerTestParameterized, invalid_message) { rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + SetUpAndActivateTrajectoryController( + true, {partial_joints_parameters, allow_integration_parameters}, &executor); trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; @@ -727,6 +729,67 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } +/// With allow_integration_in_goal_trajectories parameter trajectory missing position or velocities +/// are accepted +TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +{ + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + good_traj_msg.points[0].accelerations.resize(1); + good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position and velocity data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // All empty + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + traj_msg.points[0].accelerations.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + /** * @brief test_trajectory_replace Test replacing an existing trajectory */ From 823db20683c0a1fcae88a6860285a014ae4c3665 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 21 Apr 2022 08:17:33 +0100 Subject: [PATCH 077/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 15 +++++++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 5 +++++ 12 files changed, 66 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index fbbb5df21c..f4e6134bbc 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 24b35e8653..4e7af9a37f 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4f94c1d87e..e3eaa350d6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d606949901..a91ccde64f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a031cf2e19..479b3d6a4e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6389674f01..d19323a724 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 67c7d07de9..016797cf5d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 10615fb972..7d3f87b354 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Allow integration of states in goal trajectories (`#190 `_) + * Added position and velocity deduction to trajectory. + * Added support for deduction of states from their derivatives. +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* [JTC] Implement effort-only command interface (`#225 `_) + * Fix trajectory tolerance parameters + * Implement effort command interface for JTC + * Use auto_declare for pid params + * Set effort to 0 on deactivate +* [JTC] Variable renaming for clearer API (`#323 `_) +* Remove unused include to fix JTC test (`#319 `_) +* Contributors: Akash, Andy Zelenak, Bence Magyar, Denis Štogl, Jafar Abdi, Victor Lopez + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1c7c243ff1..86a79c24bc 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index f06ebfbb86..2b613836dd 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.0 (2022-03-25) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d5f1f6337b..debd494846 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.2.0 (2022-03-25) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index a704ea8c01..5151b86bd4 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.2.0 (2022-03-25) ------------------ * Use lifecycle node as base for controllers (`#244 `_) From 109d889612f083acfe3b6f0b953397a730f2e19c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 21 Apr 2022 08:18:39 +0100 Subject: [PATCH 078/524] 2.3.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index f4e6134bbc..83462dc7a1 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 2d8c76cc95..856ed8fedc 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.2.0 + 2.3.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 4e7af9a37f..a236795497 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index b7d11c25f7..10eb423863 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.2.0 + 2.3.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 e3eaa350d6..9f584fe2a2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 564ae7d425..2e6acf0cdb 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.2.0 + 2.3.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 a91ccde64f..85bc175d2c 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 78e259fa9a..dc1027f730 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.2.0 + 2.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 479b3d6a4e..ba91ff0405 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index cb8a1f416b..d2b63350ce 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.2.0 + 2.3.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d19323a724..319cfe3ba4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5b1e6251b5..5764c32c57 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.2.0 + 2.3.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 016797cf5d..abfccc542d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 208138fd19..906014d3cf 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.2.0 + 2.3.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 7d3f87b354..1db5caa5ed 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * [JTC] Allow integration of states in goal trajectories (`#190 `_) * Added position and velocity deduction to trajectory. * Added support for deduction of states from their derivatives. diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index e6409bd71b..fe6e9b37e0 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.2.0 + 2.3.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 86a79c24bc..c792af3d7f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 92b91a6955..11379d90d6 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.2.0 + 2.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2b613836dd..b6a9738f55 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ 2.2.0 (2022-03-25) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 92e92b89a8..49afb78ac5 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.2.0 + 2.3.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 debd494846..2e158d7594 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ 2.2.0 (2022-03-25) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2b91fb6d0e..f824f0051b 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.2.0 + 2.3.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 5151b86bd4..2a0477997d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.0 (2022-04-21) +------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e60cf7b4bb..66375467e0 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.2.0 + 2.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2a685437b43590addcb1c3e65c15fd6f80e26642 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Sat, 23 Apr 2022 23:38:51 -0700 Subject: [PATCH 079/524] Fix JTC state tolerance and goal_time tolerance check bug (#316) * fix state tolerance check bug * hold position when canceling or aborting. update state tolerance test * add goal tolerance fail test * better state tolerance test * use predefined constants * fix goal_time logic and tests * add comments Co-authored-by: Michael Wiznitzer --- .../src/joint_trajectory_controller.cpp | 161 ++++++++++-------- .../test/test_trajectory_actions.cpp | 70 +++++++- 2 files changed, 154 insertions(+), 77 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0af506b83e..cdbf138b82 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -188,52 +188,11 @@ controller_interface::return_type JointTrajectoryController::update( { bool abort = false; bool outside_goal_tolerance = false; + bool within_goal_time = true; + double time_difference = 0.0; const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - // set values for next hardware write() - if (use_closed_loop_pid_adapter) - { - // Update PIDs - for (auto i = 0ul; i < joint_num; ++i) - { - tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_desired.positions[i] - state_current.positions[i], - state_desired.velocities[i] - state_current.velocities[i], - (uint64_t)period.nanoseconds()); - } - } - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired.positions); - } - if (has_velocity_command_interface_) - { - if (use_closed_loop_pid_adapter) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); - } - if (has_effort_command_interface_) - { - if (use_closed_loop_pid_adapter) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired.effort); - } - } - + // Check state/goal tolerance for (size_t index = 0; index < joint_num; ++index) { compute_error_for_joint(state_error, index, state_current, state_desired); @@ -252,11 +211,74 @@ controller_interface::return_type JointTrajectoryController::update( state_error, index, default_tolerances_.goal_state_tolerance[index], false)) { outside_goal_tolerance = true; + + if (default_tolerances_.goal_time_tolerance != 0.0) + { + // if we exceed goal_time_tolerance set it to aborted + const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; + + time_difference = node_->now().seconds() - traj_end.seconds(); + + if (time_difference > default_tolerances_.goal_time_tolerance) + { + within_goal_time = false; + } + } } } - // store command as state when hardware state has tracking offset - last_commanded_state_ = state_desired; + // set values for next hardware write() if tolerance is met + if (!abort && within_goal_time) + { + if (use_closed_loop_pid_adapter) + { + // Update PIDs + for (auto i = 0ul; i < joint_num; ++i) + { + tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired.positions[i] - state_current.positions[i], + state_desired.velocities[i] - state_current.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + } + } + + // store command as state when hardware state has tracking offset + last_commanded_state_ = state_desired; + } const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) @@ -272,20 +294,13 @@ controller_interface::return_type JointTrajectoryController::update( active_goal->setFeedback(feedback); // check abort - if (abort || outside_goal_tolerance) + if (abort) { + set_hold_position(); auto result = std::make_shared(); - if (abort) - { - RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - } - else if (outside_goal_tolerance) - { - RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - } + RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 @@ -306,26 +321,21 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); } - else if (default_tolerances_.goal_time_tolerance != 0.0) + else if (!within_goal_time) { - // if we exceed goal_time_toleralance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - const double difference = time.seconds() - traj_end.seconds(); - if (difference > default_tolerances_.goal_time_tolerance) - { - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN( - node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - difference); - } + set_hold_position(); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + RCLCPP_WARN( + node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + time_difference); } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied or violated within the goal_time_tolerance } } } @@ -1253,6 +1263,7 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { + set_hold_position(); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index a7926d2278..fe65711aef 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -382,12 +382,71 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) SetUpExecutor(params); SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, + common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); +} + +TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) +{ + // set joint tolerance parameters + const double goal_tol = 0.1; + // set very small goal_time so that goal_time is violated + const double goal_time = 0.000001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + const double init_pos1 = joint_pos_[0]; + const double init_pos2 = joint_pos_[1]; + const double init_pos3 = joint_pos_[2]; + std::shared_future gh_future; // send goal { std::vector points; JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(1.0); + point.time_from_start = rclcpp::Duration::from_seconds(0.0); point.positions.resize(joint_names_.size()); point.positions[0] = 4.0; @@ -402,8 +461,15 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, + control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); } TEST_F(TestTrajectoryActions, test_cancel_hold_position) From 53d3b25564ac82211af8c24ef47d47169015f6dd Mon Sep 17 00:00:00 2001 From: Jack Date: Mon, 25 Apr 2022 03:56:22 -0600 Subject: [PATCH 080/524] updated to use node getter functions (#329) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * updated to use node getter functions * updated get_node_const() name Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- .../src/diff_drive_controller.cpp | 142 +++++++++--------- .../src/force_torque_sensor_broadcaster.cpp | 26 ++-- .../src/forward_command_controller.cpp | 10 +- .../gripper_action_controller_impl.hpp | 58 +++---- .../src/imu_sensor_broadcaster.cpp | 14 +- .../src/joint_state_broadcaster.cpp | 4 +- .../src/joint_trajectory_controller.cpp | 91 +++++------ 7 files changed, 178 insertions(+), 167 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index cee17d01f1..e1f0e432df 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -142,7 +142,7 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto logger = node_->get_logger(); + auto logger = get_node()->get_logger(); if (get_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) @@ -299,11 +299,11 @@ controller_interface::return_type DiffDriveController::update( controller_interface::CallbackReturn DiffDriveController::on_configure( const rclcpp_lifecycle::State &) { - auto logger = node_->get_logger(); + auto logger = get_node()->get_logger(); // update parameters - left_wheel_names_ = node_->get_parameter("left_wheel_names").as_string_array(); - right_wheel_names_ = node_->get_parameter("right_wheel_names").as_string_array(); + left_wheel_names_ = get_node()->get_parameter("left_wheel_names").as_string_array(); + right_wheel_names_ = get_node()->get_parameter("right_wheel_names").as_string_array(); if (left_wheel_names_.size() != right_wheel_names_.size()) { @@ -319,16 +319,16 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - wheel_params_.separation = node_->get_parameter("wheel_separation").as_double(); + wheel_params_.separation = get_node()->get_parameter("wheel_separation").as_double(); wheel_params_.wheels_per_side = - static_cast(node_->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = node_->get_parameter("wheel_radius").as_double(); + static_cast(get_node()->get_parameter("wheels_per_side").as_int()); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); wheel_params_.separation_multiplier = - node_->get_parameter("wheel_separation_multiplier").as_double(); + get_node()->get_parameter("wheel_separation_multiplier").as_double(); wheel_params_.left_radius_multiplier = - node_->get_parameter("left_wheel_radius_multiplier").as_double(); + get_node()->get_parameter("left_wheel_radius_multiplier").as_double(); wheel_params_.right_radius_multiplier = - node_->get_parameter("right_wheel_radius_multiplier").as_double(); + get_node()->get_parameter("right_wheel_radius_multiplier").as_double(); const auto wheels = wheel_params_; @@ -338,62 +338,62 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize( - node_->get_parameter("velocity_rolling_window_size").as_int()); + get_node()->get_parameter("velocity_rolling_window_size").as_int()); - odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); + odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); + auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); std::copy( pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); + auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); std::copy( twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); - odom_params_.position_feedback = node_->get_parameter("position_feedback").as_bool(); - odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); + odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); + odom_params_.position_feedback = get_node()->get_parameter("position_feedback").as_bool(); + odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = node_->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); + static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; + publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); try { limiter_linear_ = SpeedLimiter( - node_->get_parameter("linear.x.has_velocity_limits").as_bool(), - node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), - node_->get_parameter("linear.x.has_jerk_limits").as_bool(), - node_->get_parameter("linear.x.min_velocity").as_double(), - node_->get_parameter("linear.x.max_velocity").as_double(), - node_->get_parameter("linear.x.min_acceleration").as_double(), - node_->get_parameter("linear.x.max_acceleration").as_double(), - node_->get_parameter("linear.x.min_jerk").as_double(), - node_->get_parameter("linear.x.max_jerk").as_double()); + get_node()->get_parameter("linear.x.has_velocity_limits").as_bool(), + get_node()->get_parameter("linear.x.has_acceleration_limits").as_bool(), + get_node()->get_parameter("linear.x.has_jerk_limits").as_bool(), + get_node()->get_parameter("linear.x.min_velocity").as_double(), + get_node()->get_parameter("linear.x.max_velocity").as_double(), + get_node()->get_parameter("linear.x.min_acceleration").as_double(), + get_node()->get_parameter("linear.x.max_acceleration").as_double(), + get_node()->get_parameter("linear.x.min_jerk").as_double(), + get_node()->get_parameter("linear.x.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR(node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring linear speed limiter: %s", e.what()); } try { limiter_angular_ = SpeedLimiter( - node_->get_parameter("angular.z.has_velocity_limits").as_bool(), - node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), - node_->get_parameter("angular.z.has_jerk_limits").as_bool(), - node_->get_parameter("angular.z.min_velocity").as_double(), - node_->get_parameter("angular.z.max_velocity").as_double(), - node_->get_parameter("angular.z.min_acceleration").as_double(), - node_->get_parameter("angular.z.max_acceleration").as_double(), - node_->get_parameter("angular.z.min_jerk").as_double(), - node_->get_parameter("angular.z.max_jerk").as_double()); + get_node()->get_parameter("angular.z.has_velocity_limits").as_bool(), + get_node()->get_parameter("angular.z.has_acceleration_limits").as_bool(), + get_node()->get_parameter("angular.z.has_jerk_limits").as_bool(), + get_node()->get_parameter("angular.z.min_velocity").as_double(), + get_node()->get_parameter("angular.z.max_velocity").as_double(), + get_node()->get_parameter("angular.z.min_acceleration").as_double(), + get_node()->get_parameter("angular.z.max_acceleration").as_double(), + get_node()->get_parameter("angular.z.min_jerk").as_double(), + get_node()->get_parameter("angular.z.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR(node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } if (!reset()) @@ -407,7 +407,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( if (publish_limited_velocity_) { limited_velocity_publisher_ = - node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = std::make_shared>(limited_velocity_publisher_); } @@ -422,46 +422,49 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // initialize command subscriber if (use_stamped_vel_) { - velocity_command_subscriber_ = node_->create_subscription( + velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { - RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + 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( - node_->get_logger(), + get_node()->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " "time, this message will only be shown once"); - msg->header.stamp = node_->get_clock()->now(); + msg->header.stamp = get_node()->get_clock()->now(); } received_velocity_msg_ptr_.set(std::move(msg)); }); } else { - velocity_command_unstamped_subscriber_ = node_->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) - { - RCLCPP_WARN(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 = node_->get_clock()->now(); - }); + 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(); + }); } // initialize odometry publisher and messasge - odometry_publisher_ = node_->create_publisher( + odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = std::make_shared>( @@ -472,9 +475,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message.child_frame_id = odom_params_.base_frame_id; // limit the publication on the topics /odom and /tf - publish_rate_ = node_->get_parameter("publish_rate").as_double(); + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - previous_publish_timestamp_ = node_->get_clock()->now(); + previous_publish_timestamp_ = get_node()->get_clock()->now(); // initialize odom values zeros odometry_message.twist = @@ -491,7 +494,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // initialize transform publisher and message - odometry_transform_publisher_ = node_->create_publisher( + odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_transform_publisher_ = std::make_shared>( @@ -503,7 +506,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; - previous_update_timestamp_ = node_->get_clock()->now(); + previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; } @@ -525,14 +528,15 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( - node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); + get_node()->get_logger(), + "Either left wheel interfaces, right wheel interfaces are non existent"); return controller_interface::CallbackReturn::ERROR; } is_halted = false; subscriber_is_active_ = true; - RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); return controller_interface::CallbackReturn::SUCCESS; } @@ -607,7 +611,7 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles) { - auto logger = node_->get_logger(); + auto logger = get_node()->get_logger(); if (wheel_names.empty()) { diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e79ec4b007..e88ef51cd2 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -53,13 +53,13 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); - interface_names_[0] = node_->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = node_->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = node_->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = node_->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string(); + sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); + interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string(); + interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string(); + interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string(); + interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string(); + interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string(); + interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string(); const bool no_interface_names_defined = std::count(interface_names_.begin(), interface_names_.end(), "") == 6; @@ -67,7 +67,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (sensor_name_.empty() && no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); return controller_interface::CallbackReturn::ERROR; @@ -76,16 +76,16 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (!sensor_name_.empty() && !no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); return controller_interface::CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = get_node()->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return controller_interface::CallbackReturn::ERROR; } @@ -105,7 +105,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( try { // register ft sensor data publisher - sensor_state_publisher_ = node_->create_publisher( + sensor_state_publisher_ = get_node()->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } @@ -121,7 +121,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index fc9564b9be..bbe2780e01 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -57,7 +57,7 @@ controller_interface::CallbackReturn ForwardCommandController::on_init() controller_interface::CallbackReturn ForwardCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = node_->get_parameter("joints").as_string_array(); + joint_names_ = get_node()->get_parameter("joints").as_string_array(); if (joint_names_.empty()) { @@ -68,7 +68,7 @@ controller_interface::CallbackReturn ForwardCommandController::on_configure( // Specialized, child controllers set interfaces before calling configure function. if (interface_name_.empty()) { - interface_name_ = node_->get_parameter("interface_name").as_string(); + interface_name_ = get_node()->get_parameter("interface_name").as_string(); } if (interface_name_.empty()) @@ -118,8 +118,8 @@ controller_interface::CallbackReturn ForwardCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), - ordered_interfaces.size()); + get_node()->get_logger(), "Expected %zu position command interfaces, got %zu", + joint_names_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } @@ -151,7 +151,7 @@ controller_interface::return_type ForwardCommandController::update( if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *node_->get_clock(), 1000, + get_node()->get_logger(), *get_node()->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 42aee83b4c..2cd66776b5 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -70,7 +70,7 @@ controller_interface::return_type GripperActionController::up const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; - check_for_success(node_->now(), error_position, current_position, current_velocity); + check_for_success(get_node()->now(), error_position, current_position, current_velocity); // Hardware interface adapter: Generate and send commands computed_command_ = hw_iface_adapter_.updateCommand( @@ -83,7 +83,7 @@ template rclcpp_action::GoalResponse GripperActionController::goal_callback( const rclcpp_action::GoalUUID &, std::shared_ptr) { - RCLCPP_INFO(node_->get_logger(), "Received & accepted new action goal"); + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -106,12 +106,12 @@ void GripperActionController::accepted_callback( pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false; - last_movement_time_ = node_->now(); + last_movement_time_ = get_node()->now(); rt_active_goal_ = rt_goal; rt_active_goal_->execute(); } // Setup goal status checking timer - goal_handle_timer_ = node_->create_wall_timer( + goal_handle_timer_ = get_node()->create_wall_timer( action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } @@ -120,7 +120,7 @@ template rclcpp_action::CancelResponse GripperActionController::cancel_callback( const std::shared_ptr goal_handle) { - RCLCPP_INFO(node_->get_logger(), "Got request to cancel goal"); + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) @@ -129,7 +129,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel set_hold_position(); RCLCPP_INFO( - node_->get_logger(), "Canceling active action goal because cancel callback received."); + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -189,17 +189,17 @@ template controller_interface::CallbackReturn GripperActionController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = node_->get_logger(); + const auto logger = get_node()->get_logger(); // Action status checking update rate - const auto action_monitor_rate = node_->get_parameter("action_monitor_rate").as_double(); - action_monitor_period_ = - rclcpp::Duration::from_seconds(1.0 / node_->get_parameter("action_monitor_rate").as_double()); + const auto action_monitor_rate = get_node()->get_parameter("action_monitor_rate").as_double(); + action_monitor_period_ = rclcpp::Duration::from_seconds( + 1.0 / get_node()->get_parameter("action_monitor_rate").as_double()); RCLCPP_INFO_STREAM( logger, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); // Controlled joint - joint_name_ = node_->get_parameter("joint").as_string(); + joint_name_ = get_node()->get_parameter("joint").as_string(); if (joint_name_.empty()) { RCLCPP_ERROR(logger, "Could not find joint name on param server"); @@ -207,14 +207,14 @@ controller_interface::CallbackReturn GripperActionController: } // Default tolerances - goal_tolerance_ = node_->get_parameter("goal_tolerance").as_double(); + goal_tolerance_ = get_node()->get_parameter("goal_tolerance").as_double(); goal_tolerance_ = fabs(goal_tolerance_); // Max allowable effort - default_max_effort_ = node_->get_parameter("max_effort").as_double(); + default_max_effort_ = get_node()->get_parameter("max_effort").as_double(); default_max_effort_ = fabs(default_max_effort_); // Stall - stall velocity threshold, stall timeout - stall_velocity_threshold_ = node_->get_parameter("stall_velocity_threshold").as_double(); - stall_timeout_ = node_->get_parameter("stall_timeout").as_double(); + stall_velocity_threshold_ = get_node()->get_parameter("stall_velocity_threshold").as_double(); + stall_timeout_ = get_node()->get_parameter("stall_timeout").as_double(); return controller_interface::CallbackReturn::SUCCESS; } @@ -229,15 +229,15 @@ controller_interface::CallbackReturn GripperActionController: }); if (position_command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR(node_->get_logger(), "Expected 1 position command interface"); + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); return controller_interface::CallbackReturn::ERROR; } if (position_command_interface_it->get_name() != joint_name_) { RCLCPP_ERROR_STREAM( - node_->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_name() << "` != `" << joint_name_ - << "`"); + get_node()->get_logger(), "Position command interface is different than joint name `" + << position_command_interface_it->get_name() << "` != `" + << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -247,15 +247,15 @@ controller_interface::CallbackReturn GripperActionController: }); if (position_state_interface_it == state_interfaces_.end()) { - RCLCPP_ERROR(node_->get_logger(), "Expected 1 position state interface"); + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); return controller_interface::CallbackReturn::ERROR; } if (position_state_interface_it->get_name() != joint_name_) { RCLCPP_ERROR_STREAM( - node_->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_name() << "` != `" << joint_name_ - << "`"); + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_name() << "` != `" + << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -265,15 +265,15 @@ controller_interface::CallbackReturn GripperActionController: }); if (velocity_state_interface_it == state_interfaces_.end()) { - RCLCPP_ERROR(node_->get_logger(), "Expected 1 velocity state interface"); + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); return controller_interface::CallbackReturn::ERROR; } if (velocity_state_interface_it->get_name() != joint_name_) { RCLCPP_ERROR_STREAM( - node_->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_name() << "` != `" << joint_name_ - << "`"); + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_name() << "` != `" + << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -282,7 +282,7 @@ controller_interface::CallbackReturn GripperActionController: joint_velocity_state_interface_ = *velocity_state_interface_it; // Hardware interface adapter - hw_iface_adapter_.init(joint_position_command_interface_, node_); + hw_iface_adapter_.init(joint_position_command_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -296,7 +296,7 @@ controller_interface::CallbackReturn GripperActionController: // Action interface action_server_ = rclcpp_action::create_server( - node_, "~/gripper_cmd", + get_node(), "~/gripper_cmd", std::bind( &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 4e88960995..b609bca8c7 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -33,7 +33,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() catch (const std::exception & e) { RCLCPP_ERROR( - node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -43,17 +43,17 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); + sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); if (sensor_name_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'sensor_name' parameter has to be specified."); + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = get_node()->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -63,7 +63,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( { // register ft sensor data publisher sensor_state_publisher_ = - node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception & e) @@ -78,7 +78,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 83656f10f6..a1ad76b092 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -183,7 +183,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( if (!init_joint_data()) { RCLCPP_ERROR( - node_->get_logger(), "None of requested interfaces exist. Controller will not run."); + get_node()->get_logger(), "None of requested interfaces exist. Controller will not run."); return CallbackReturn::ERROR; } @@ -195,7 +195,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( state_interfaces_.size() != (joints_.size() * interfaces_.size())) { RCLCPP_WARN( - node_->get_logger(), + get_node()->get_logger(), "Not all requested interfaces exists. " "Check ControllerManager output for more detailed information."); } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cdbf138b82..400595fdce 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -218,7 +218,7 @@ controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - time_difference = node_->now().seconds() - traj_end.seconds(); + time_difference = get_node()->now().seconds() - traj_end.seconds(); if (time_difference > default_tolerances_.goal_time_tolerance) { @@ -299,7 +299,7 @@ controller_interface::return_type JointTrajectoryController::update( set_hold_position(); auto result = std::make_shared(); - RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here @@ -319,7 +319,7 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); } else if (!within_goal_time) { @@ -331,7 +331,7 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", time_difference); } // else, run another cycle while waiting for outside_goal_tolerance @@ -453,10 +453,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = node_->get_logger(); + const auto logger = get_node()->get_logger(); // update parameters - joint_names_ = node_->get_parameter("joints").as_string_array(); + joint_names_ = get_node()->get_parameter("joints").as_string_array(); if (!reset()) { @@ -471,7 +471,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { - command_interface_types_ = node_->get_parameter("command_interfaces").as_string_array(); + command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); } if (command_interface_types_.empty()) @@ -572,7 +572,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Read always state interfaces from the parameter because they can be used // independently from the controller's type. // Specialized, child controllers should set its default value. - state_interface_types_ = node_->get_parameter("state_interfaces").as_string_array(); + state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); if (state_interface_types_.empty()) { @@ -667,12 +667,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*node_, joint_names_); + default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); // Read parameters customizing controller for special cases - open_loop_control_ = node_->get_parameter("open_loop_control").get_value(); + open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); allow_integration_in_goal_trajectories_ = - node_->get_parameter("allow_integration_in_goal_trajectories").get_value(); + get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); // subscriber callback // non realtime @@ -692,14 +692,16 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( }; // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = node_->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); + joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = node_->get_parameter("state_publish_rate").get_value(); + const double state_publish_rate = + get_node()->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); if (state_publish_rate > 0.0) { @@ -710,7 +712,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); const auto n_joints = joint_names_.size(); @@ -734,26 +737,27 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } state_publisher_->unlock(); - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = get_node()->now(); // action server configuration - allow_partial_joints_goal_ = node_->get_parameter("allow_partial_joints_goal").get_value(); + allow_partial_joints_goal_ = + get_node()->get_parameter("allow_partial_joints_goal").get_value(); if (allow_partial_joints_goal_) { RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } const double action_monitor_rate = - node_->get_parameter("action_monitor_rate").get_value(); + get_node()->get_parameter("action_monitor_rate").get_value(); RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), node_->get_node_clock_interface(), - node_->get_node_logging_interface(), node_->get_node_waitables_interface(), - std::string(node_->get_name()) + "/follow_joint_trajectory", + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/follow_joint_trajectory", std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); @@ -774,8 +778,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_command_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", + joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -788,8 +792,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_state_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -815,7 +819,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); @@ -931,14 +935,14 @@ void JointTrajectoryController::publish_state( return; } - if (node_->now() < (last_state_publish_time_ + state_publisher_period_)) + if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) { return; } if (state_publisher_ && state_publisher_->trylock()) { - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = get_node()->now(); state_publisher_->msg_.header.stamp = last_state_publish_time_; state_publisher_->msg_.desired.positions = desired_state.positions; state_publisher_->msg_.desired.velocities = desired_state.velocities; @@ -963,12 +967,13 @@ void JointTrajectoryController::publish_state( rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - RCLCPP_INFO(node_->get_logger(), "Received new action goal"); + RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); // Precondition: Running controller if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } @@ -977,14 +982,14 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::REJECT; } - RCLCPP_INFO(node_->get_logger(), "Accepted new action goal"); + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( const std::shared_ptr> goal_handle) { - RCLCPP_INFO(node_->get_logger(), "Got request to cancel goal"); + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) const auto active_goal = *rt_active_goal_.readFromNonRT(); @@ -995,7 +1000,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( set_hold_position(); RCLCPP_DEBUG( - node_->get_logger(), "Canceling active action goal because cancel callback received."); + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -1024,7 +1029,7 @@ void JointTrajectoryController::feedback_setup_callback( rt_active_goal_.writeFromNonRT(rt_goal); // Setup goal status checking timer - goal_handle_timer_ = node_->create_wall_timer( + goal_handle_timer_ = get_node()->create_wall_timer( action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } @@ -1101,7 +1106,8 @@ void JointTrajectoryController::sort_to_local_joint_order( } if (to_remap.size() != mapping.size()) { - RCLCPP_WARN(node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + RCLCPP_WARN( + get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } std::vector output; @@ -1141,7 +1147,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } @@ -1157,14 +1163,15 @@ bool JointTrajectoryController::validate_trajectory_msg( if (trajectory.joint_names.size() != joint_names_.size()) { RCLCPP_ERROR( - node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); + get_node()->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); return false; } } if (trajectory.joint_names.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Empty joint names on incoming trajectory."); + RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } @@ -1179,10 +1186,10 @@ bool JointTrajectoryController::validate_trajectory_msg( { trajectory_end_time += p.time_from_start; } - if (trajectory_end_time < node_->now()) + if (trajectory_end_time < get_node()->now()) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "Received trajectory with non zero time start time (%f) that ends on the past (%f)", trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; @@ -1197,7 +1204,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if (it == joint_names_.end()) { RCLCPP_ERROR( - node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", + get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", incoming_joint_name.c_str()); return false; } @@ -1209,7 +1216,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", i - 1, i, previous_traj_time.seconds(), rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); From 7acf29c191ea44870c7327436a4579e06261fde9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Apr 2022 14:52:41 +0200 Subject: [PATCH 081/524] Multi-interface Forward Controller (#154) * ForwardController prepared for inheritance * Added MultiInterfaceForwardCommandController and restructured forward_controller package. * Optimize tests of multi-interface command controller. --- .../src/joint_group_effort_controller.cpp | 1 - forward_command_controller/CMakeLists.txt | 23 ++ .../forward_command_plugin.xml | 6 + .../forward_command_controller.hpp | 48 +-- .../forward_controllers_base.hpp | 106 ++++++ ...i_interface_forward_command_controller.hpp | 54 +++ .../src/forward_command_controller.cpp | 114 +----- .../src/forward_controllers_base.cpp | 148 ++++++++ ...i_interface_forward_command_controller.cpp | 64 ++++ .../test/test_forward_command_controller.cpp | 57 ++- ...i_interface_forward_command_controller.cpp | 41 ++ ...i_interface_forward_command_controller.cpp | 355 ++++++++++++++++++ ...i_interface_forward_command_controller.hpp | 82 ++++ .../src/joint_group_position_controller.cpp | 1 - .../src/joint_group_velocity_controller.cpp | 1 - 15 files changed, 933 insertions(+), 168 deletions(-) create mode 100644 forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp create mode 100644 forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp create mode 100644 forward_command_controller/src/forward_controllers_base.cpp create mode 100644 forward_command_controller/src/multi_interface_forward_command_controller.cpp create mode 100644 forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp create mode 100644 forward_command_controller/test/test_multi_interface_forward_command_controller.cpp create mode 100644 forward_command_controller/test/test_multi_interface_forward_command_controller.hpp diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index b049292fe0..c3748d493c 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -25,7 +25,6 @@ namespace effort_controllers JointGroupEffortController::JointGroupEffortController() : forward_command_controller::ForwardCommandController() { - logger_name_ = "joint effort controller"; interface_name_ = hardware_interface::HW_IF_EFFORT; } diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index a600733a94..2490b5c856 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -22,7 +22,9 @@ find_package(std_msgs REQUIRED) add_library(forward_command_controller SHARED + src/forward_controllers_base.cpp src/forward_command_controller.cpp + src/multi_interface_forward_command_controller.cpp ) target_include_directories(forward_command_controller PRIVATE include) ament_target_dependencies(forward_command_controller @@ -78,6 +80,27 @@ if(BUILD_TESTING) target_link_libraries(test_forward_command_controller forward_command_controller ) + + ament_add_gmock( + test_load_multi_interface_forward_command_controller + test/test_load_multi_interface_forward_command_controller.cpp + ) + target_include_directories(test_load_multi_interface_forward_command_controller PRIVATE include) + ament_target_dependencies( + test_load_multi_interface_forward_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_multi_interface_forward_command_controller + test/test_multi_interface_forward_command_controller.cpp + ) + target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) + target_link_libraries(test_multi_interface_forward_command_controller + forward_command_controller + ) endif() ament_export_dependencies( diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index f8b1261d6d..48fa29936d 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -4,4 +4,10 @@ The forward command controller commands a group of joints in a given interface + + + MultiInterfaceForwardController ros2_control controller. + + diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 5a1da3d40e..f50af328d8 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -15,27 +15,18 @@ #ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ -#include #include #include -#include "controller_interface/controller_interface.hpp" +#include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" -#include "rclcpp/subscription.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller { -using CmdType = std_msgs::msg::Float64MultiArray; - /** * \brief Forward command controller for a set of joints. * - * This class forwards the command signal down to a set of joints - * on the specified interface. + * This class forwards the command signal down to a set of joints on the specified interface. * * \param joints Names of the joints to control. * \param interface_name Name of the interface to command. @@ -43,45 +34,18 @@ using CmdType = std_msgs::msg::Float64MultiArray; * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class ForwardCommandController : public controller_interface::ControllerInterface +class ForwardCommandController : public ForwardControllersBase { public: FORWARD_COMMAND_CONTROLLER_PUBLIC ForwardCommandController(); - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; + std::vector joint_names_; std::string interface_name_; - - realtime_tools::RealtimeBuffer> rt_command_ptr_; - rclcpp::Subscription::SharedPtr joints_command_subscriber_; - - std::string logger_name_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp new file mode 100644 index 0000000000..3e153d7e2e --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_ +#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "forward_command_controller/visibility_control.h" +#include "rclcpp/subscription.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace forward_command_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; + +/** + * \brief Forward command controller for a set of joints and interfaces. + * + * This class forwards the command signal down to a set of joints or interfaces. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class ForwardControllersBase : public controller_interface::ControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardControllersBase(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + ~ForwardControllersBase() = default; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + /** + * Derived controllers have to declare parameters in this method. + * Error handling does not have to be done. It is done in `on_init`-method of this class. + */ + virtual void declare_parameters() = 0; + + /** + * Derived controllers have to read parameters in this method and set `command_interface_types_` + * variable. The variable is then used to propagate the command interface configuration to + * controller manager. The method is called from `on_configure`-method of this class. + * + * It is expected that error handling of exceptions is done. + * + * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are + * allowed, controller_interface::CallbackReturn::ERROR otherwise. + */ + virtual controller_interface::CallbackReturn read_parameters() = 0; + + std::vector joint_names_; + std::string interface_name_; + + std::vector command_interface_types_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp new file mode 100644 index 0000000000..8acec36c5c --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ + +#include +#include + +#include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/visibility_control.h" + +namespace forward_command_controller +{ +/** + * \brief Multi interface forward command controller for a set of interfaces. + * + * This class forwards the command signal down to a set of interfaces on the specified joint. + * + * \param joint Name of the joint to control. + * \param interface_names Names of the interfaces to command. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class MultiInterfaceForwardCommandController +: public forward_command_controller::ForwardControllersBase +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + MultiInterfaceForwardCommandController(); + +protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; + + std::string joint_name_; + std::vector interface_names_; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index bbe2780e01..46e90cd1c4 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -20,42 +20,20 @@ #include #include -#include "controller_interface/helpers.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" -#include "hardware_interface/loaned_command_interface.hpp" - namespace forward_command_controller { -using hardware_interface::LoanedCommandInterface; - -ForwardCommandController::ForwardCommandController() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) -{ -} +ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {} -controller_interface::CallbackReturn ForwardCommandController::on_init() +void ForwardCommandController::declare_parameters() { - try - { - auto_declare>("joints", std::vector()); - - auto_declare("interface_name", ""); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; + get_node()->declare_parameter>("joints", std::vector()); + get_node()->declare_parameter("interface_name", ""); } -controller_interface::CallbackReturn ForwardCommandController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn ForwardCommandController::read_parameters() { joint_names_ = get_node()->get_parameter("joints").as_string_array(); @@ -77,94 +55,14 @@ controller_interface::CallbackReturn ForwardCommandController::on_configure( return controller_interface::CallbackReturn::ERROR; } - joints_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -ForwardCommandController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint : joint_names_) { - command_interfaces_config.names.push_back(joint + "/" + interface_name_); - } - - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration -ForwardCommandController::state_interface_configuration() const -{ - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; -} - -controller_interface::CallbackReturn ForwardCommandController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // check if we have all resources defined in the "points" parameter - // also verify that we *only* have the resources defined in the "points" parameter - std::vector> ordered_interfaces; - if ( - !controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface_name_, ordered_interfaces) || - command_interfaces_.size() != ordered_interfaces.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu position command interfaces, got %zu", - joint_names_.size(), ordered_interfaces.size()); - return controller_interface::CallbackReturn::ERROR; + command_interface_types_.push_back(joint + "/" + interface_name_); } - // reset command buffer if a command came through callback when controller was inactive - rt_command_ptr_.reset(); - return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardCommandController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // reset command buffer - rt_command_ptr_.reset(); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type ForwardCommandController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - auto joint_commands = rt_command_ptr_.readFromRT(); - - // no command received yet - if (!joint_commands || !(*joint_commands)) - { - return controller_interface::return_type::OK; - } - - if ((*joint_commands)->data.size() != command_interfaces_.size()) - { - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*joint_commands)->data.size(), command_interfaces_.size()); - return controller_interface::return_type::ERROR; - } - - for (size_t index = 0; index < command_interfaces_.size(); ++index) - { - command_interfaces_[index].set_value((*joint_commands)->data[index]); - } - - return controller_interface::return_type::OK; -} - } // namespace forward_command_controller #include "pluginlib/class_list_macros.hpp" diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp new file mode 100644 index 0000000000..e4ea46fcc5 --- /dev/null +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -0,0 +1,148 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "forward_command_controller/forward_controllers_base.hpp" + +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace forward_command_controller +{ +ForwardControllersBase::ForwardControllersBase() +: controller_interface::ControllerInterface(), + rt_command_ptr_(nullptr), + joints_command_subscriber_(nullptr) +{ +} + +controller_interface::CallbackReturn ForwardControllersBase::on_init() +{ + try + { + declare_parameters(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardControllersBase::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = this->read_parameters(); + if (ret != controller_interface::CallbackReturn::SUCCESS) + { + return ret; + } + + joints_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +ForwardControllersBase::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn ForwardControllersBase::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + // ATTENTION(destogl): Shouldn't we use ordered interface all the time? + std::vector> + ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || + command_interface_types_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_types_.size(), ordered_interfaces.size()); + return controller_interface::CallbackReturn::ERROR; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type ForwardControllersBase::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!joint_commands || !(*joint_commands)) + { + return controller_interface::return_type::OK; + } + + if ((*joint_commands)->data.size() != command_interfaces_.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + (*joint_commands)->data.size(), command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + + for (auto index = 0ul; index < command_interfaces_.size(); ++index) + { + command_interfaces_[index].set_value((*joint_commands)->data[index]); + } + + return controller_interface::return_type::OK; +} + +} // namespace forward_command_controller diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp new file mode 100644 index 0000000000..6cc22cf549 --- /dev/null +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -0,0 +1,64 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" + +#include +#include + +namespace forward_command_controller +{ +MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() +: ForwardControllersBase() +{ +} + +void MultiInterfaceForwardCommandController::declare_parameters() +{ + get_node()->declare_parameter("joint", joint_name_); + get_node()->declare_parameter>("interface_names", interface_names_); +} + +controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() +{ + joint_name_ = get_node()->get_parameter("joint").as_string(); + interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); + + if (joint_name_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (interface_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & interface : interface_names_) + { + command_interface_types_.push_back(joint_name_ + "/" + interface); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace forward_command_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::MultiInterfaceForwardCommandController, + controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 4011f13d71..697e42d671 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -33,7 +33,6 @@ #include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; namespace @@ -77,7 +76,9 @@ TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'joints' parameter not set - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) @@ -86,7 +87,9 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) controller_->get_node()->set_parameter({"joints", std::vector()}); // configure failed, 'interface_name' parameter not set - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) @@ -97,7 +100,9 @@ TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'joints' is empty - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) @@ -107,7 +112,9 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'interface_name' is empty - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) @@ -118,7 +125,9 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -129,8 +138,12 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) controller_->get_node()->set_parameter({"interface_name", "position"}); // activate failed, 'joint4' is not a valid joint name for the hardware - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) @@ -141,8 +154,12 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) controller_->get_node()->set_parameter({"interface_name", "acceleration"}); // activate failed, 'acceleration' is not a registered interface for `joint1` - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); } TEST_F(ForwardCommandControllerTest, ActivateSuccess) @@ -153,8 +170,12 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // activate successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -164,7 +185,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) // configure controller controller_->get_node()->set_parameter({"joints", joint_names_}); controller_->get_node()->set_parameter({"interface_name", "position"}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); // update successful though no command has been send yet ASSERT_EQ( @@ -200,7 +223,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_->get_node()->set_parameter({"joints", joint_names_}); controller_->get_node()->set_parameter({"interface_name", "position"}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); // send command with wrong number of joints auto command_ptr = std::make_shared(); @@ -225,7 +250,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) // configure controller controller_->get_node()->set_parameter({"joints", joint_names_}); controller_->get_node()->set_parameter({"interface_name", "position"}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); // update successful, no command received yet ASSERT_EQ( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp new file mode 100644 index 0000000000..2f540bd0e5 --- /dev/null +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik, Inc. +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMultiInterfaceForwardController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/MultiInterfaceForwardCommandController")); +} 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 new file mode 100644 index 0000000000..0cada04859 --- /dev/null +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -0,0 +1,355 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Jack Center, Denis Stogl + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_multi_interface_forward_command_controller.hpp" + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; + +namespace +{ +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} +} // namespace + +void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void MultiInterfaceForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void MultiInterfaceForwardCommandControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } + +void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) +{ + const auto result = controller_->init("multi_interface_forward_command_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(joint_1_vel_cmd_); + command_ifs.emplace_back(joint_1_eff_cmd_); + controller_->assign_interfaces(std::move(command_ifs), {}); + + if (set_params_and_activate) + { + SetParametersAndActivateController(); + } +} + +void MultiInterfaceForwardCommandControllerTest::SetParametersAndActivateController() +{ + controller_->get_node()->set_parameter({"joint", "joint1"}); + controller_->get_node()->set_parameter( + {"interface_names", std::vector{"position", "velocity", "effort"}}); + + auto node_state = controller_->get_node()->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet) +{ + SetUpController(); + controller_->get_node()->set_parameter({"interface_names", std::vector()}); + + // configure failed, 'joint' parameter not set + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet) +{ + SetUpController(); + controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'interface_names' parameter not set + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, JointsParameterIsEmpty) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", ""}); + controller_->get_node()->set_parameter({"interface_names", std::vector()}); + + // configure failed, 'joint' is empty + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, InterfaceParameterEmpty) +{ + SetUpController(); + controller_->get_node()->set_parameter({"joint", "joint1"}); + controller_->get_node()->set_parameter({"interface_names", std::vector()}); + + // configure failed, 'interface_name' is empty + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + controller_->get_node()->set_parameter( + {"interface_names", std::vector{"position", "velocity", "effort"}}); + + // configure successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint2"}); + controller_->get_node()->set_parameter( + {"interface_names", std::vector{"position", "velocity", "effort"}}); + + // activate failed, 'joint2' is not a valid joint name for the hardware + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + controller_->get_node()->set_parameter( + {"interface_names", std::vector{"position", "velocity", "acceleration"}}); + + // activate failed, 'acceleration' is not a registered interface for `joint1` + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) +{ + SetUpController(true); + + // check joint commands are the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) +{ + SetUpController(true); + + // send command + auto command_ptr = std::make_shared(); + command_ptr->data = {10.0, 20.0, 30.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update successful, command received + ASSERT_EQ( + controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) +{ + SetUpController(true); + + // update successful, no command received yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check joint commands are still the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) +{ + SetUpController(true); + + // send command with wrong number of joints + auto command_ptr = std::make_shared(); + command_ptr->data = {10.0, 20.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update failed, command size does not match number of joints + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + + // check joint commands are still the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) +{ + SetUpController(true); + + // send a new command + rclcpp::Node test_node("test_node"); + auto command_pub = test_node.create_publisher( + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std_msgs::msg::Float64MultiArray command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + command_pub->publish(command_msg); + + // wait for command message to be passed + ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); + + // process callbacks + rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); +} + +TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) +{ + SetUpController(true); + + // send command + auto command_ptr = std::make_shared(); + command_ptr->data = {10.0, 20.0, 30.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update successful, command received + ASSERT_EQ( + controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + + auto node_state = controller_->get_node()->deactivate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // command ptr should be reset (nullptr) after deactivation - same check as in `update` + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // Controller is inactive but let's put some data into buffer (simulate callback when inactive) + auto command_msg = std::make_shared(); + command_msg->data = {5.5, 6.6, 7.7}; + controller_->rt_command_ptr_.writeFromNonRT(command_msg); + + // command ptr should be available and message should be there - same check as in `update` + ASSERT_TRUE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_TRUE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // Now activate again + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // command ptr should be reset (nullptr) after activation - same check as in `update` + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromNonRT() && + *(controller_->rt_command_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // values should not change + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + + // set commands again + controller_->rt_command_ptr_.writeFromNonRT(command_msg); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); +} diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp new file mode 100644 index 0000000000..62a4d4e981 --- /dev/null +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Jack Center, Denis Stogl + +#ifndef TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ +#define TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "forward_command_controller/multi_interface_forward_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +// subclassing and friending so we can access member variables +class FriendMultiInterfaceForwardCommandController +: public forward_command_controller::MultiInterfaceForwardCommandController +{ + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterIsEmpty); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterEmpty); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess); + + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ActivateSuccess); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest); + FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess); +}; + +class MultiInterfaceForwardCommandControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(bool set_params_and_activate = false); + void SetParametersAndActivateController(); + +protected: + std::unique_ptr controller_; + + // dummy joint state value used for tests + const std::string joint_name_ = "joint1"; + + double pos_cmd_ = 1.1; + double vel_cmd_ = 2.1; + double eff_cmd_ = 3.1; + + CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; + CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; + CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; +}; + +#endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 10829dc9c7..8335a200c4 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -25,7 +25,6 @@ namespace position_controllers JointGroupPositionController::JointGroupPositionController() : forward_command_controller::ForwardCommandController() { - logger_name_ = "joint position controller"; interface_name_ = hardware_interface::HW_IF_POSITION; } diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index f1a61d4fdd..eb55e052dc 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -25,7 +25,6 @@ namespace velocity_controllers JointGroupVelocityController::JointGroupVelocityController() : forward_command_controller::ForwardCommandController() { - logger_name_ = "joint velocity controller"; interface_name_ = hardware_interface::HW_IF_VELOCITY; } From dfec0d71a8cee8ffac726c64d3efca965ff6e439 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 26 Apr 2022 10:52:17 -0500 Subject: [PATCH 082/524] Delete unused variable in joint_traj_controller (#339) --- .../joint_trajectory_controller.hpp | 4 ---- 1 file changed, 4 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 1f5a75b4a4..29789b30d3 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 @@ -170,10 +170,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; - // The controller should be in halted state after creation otherwise memory corruption - // TODO(anyone): Is the variable relevant, since we are using lifecycle? - bool is_halted_ = true; - using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; From b7332341b56b4e408cecd986c8b0a051c80f5a63 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 26 Apr 2022 10:52:36 -0500 Subject: [PATCH 083/524] Fix a gtest deprecation warning (#341) --- joint_trajectory_controller/test/test_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7e6b6b8521..a4b429bdc0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1278,7 +1278,7 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"})))); // only effort controller -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( From 58a20471dc53ee553dd60eda37e1d20ae7d4921f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 29 Apr 2022 15:56:45 +0100 Subject: [PATCH 084/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 15 +++++++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 5 +++++ 12 files changed, 67 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 83462dc7a1..9930a7f1dd 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index a236795497..57745db963 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Multi-interface Forward Controller (`#154 `_) +* Contributors: Denis Štogl + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9f584fe2a2..55d5c85106 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 85bc175d2c..01f789f95f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Multi-interface Forward Controller (`#154 `_) +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ba91ff0405..dd9dbbb032 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 319cfe3ba4..b61f5eae23 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index abfccc542d..112fd1d5ce 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1db5caa5ed..0a46b2d19a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix a gtest deprecation warning (`#341 `_) +* Delete unused variable in joint_traj_controller (`#339 `_) +* updated to use node getter functions (`#329 `_) +* Fix JTC state tolerance and goal_time tolerance check bug (`#316 `_) + * fix state tolerance check bug + * hold position when canceling or aborting. update state tolerance test + * add goal tolerance fail test + * better state tolerance test + * use predefined constants + * fix goal_time logic and tests + * add comments +* Contributors: Andy Zelenak, Jack Center, Michael Wiznitzer, Bence Magyar, Denis Štogl + 2.3.0 (2022-04-21) ------------------ * [JTC] Allow integration of states in goal trajectories (`#190 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c792af3d7f..ab8000953d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Multi-interface Forward Controller (`#154 `_) +* Contributors: Denis Štogl + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b6a9738f55..207fdf9300 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.3.0 (2022-04-21) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 2e158d7594..e8ae63b64a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.3.0 (2022-04-21) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 2a0477997d..aefcfccbfc 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Multi-interface Forward Controller (`#154 `_) +* Contributors: Denis Štogl + 2.3.0 (2022-04-21) ------------------ * Use CallbackReturn from controller_interface namespace (`#333 `_) From 8f4dbaea1443499ebf5dc6a70eef5712f5db4b86 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 29 Apr 2022 15:57:08 +0100 Subject: [PATCH 085/524] 2.4.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 9930a7f1dd..8cc9002d9b 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 856ed8fedc..140ece7490 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.3.0 + 2.4.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 57745db963..ed9dcf94f0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * Multi-interface Forward Controller (`#154 `_) * Contributors: Denis Štogl diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 10eb423863..e4e02c06cd 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.3.0 + 2.4.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 55d5c85106..b2d91e79d1 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 2e6acf0cdb..f9b7e1893a 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.3.0 + 2.4.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 01f789f95f..367d24313d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * Multi-interface Forward Controller (`#154 `_) * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index dc1027f730..af1d261235 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.3.0 + 2.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index dd9dbbb032..ed6a931618 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d2b63350ce..8466acc195 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.3.0 + 2.4.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b61f5eae23..3a1cde9389 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5764c32c57..50db89e5a2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.3.0 + 2.4.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 112fd1d5ce..ca8e5ab151 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * updated to use node getter functions (`#329 `_) * Contributors: Bence Magyar, Denis Štogl, Jack Center diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 906014d3cf..4fd6178f9c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.3.0 + 2.4.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 0a46b2d19a..2d8d3ae6f6 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * Fix a gtest deprecation warning (`#341 `_) * Delete unused variable in joint_traj_controller (`#339 `_) * updated to use node getter functions (`#329 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fe6e9b37e0..1c13b114af 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.3.0 + 2.4.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 ab8000953d..e2f56c4429 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * Multi-interface Forward Controller (`#154 `_) * Contributors: Denis Štogl diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 11379d90d6..63e258f0e7 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.3.0 + 2.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 207fdf9300..69f0341400 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ 2.3.0 (2022-04-21) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 49afb78ac5..9deb6a5e49 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.3.0 + 2.4.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 e8ae63b64a..abb3886655 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ 2.3.0 (2022-04-21) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f824f0051b..011e50b5c5 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.3.0 + 2.4.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index aefcfccbfc..3fd3fa9879 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.0 (2022-04-29) +------------------ * Multi-interface Forward Controller (`#154 `_) * Contributors: Denis Štogl diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 66375467e0..ea7c332610 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.3.0 + 2.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From cad185cd392f2b5f42dc36172cfb6ba9d8056bd7 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Mon, 2 May 2022 23:23:32 -0700 Subject: [PATCH 086/524] check for nans in command interface (#346) Co-authored-by: Michael Wiznitzer --- .../src/joint_trajectory_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 400595fdce..eecbecbb04 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1064,7 +1064,8 @@ void JointTrajectoryController::fill_partial_goal( // Assume hold position with 0 velocity and acceleration for missing joints if (!it.positions.empty()) { - if (has_position_command_interface_) + if (has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) { // copy last command if cmd interface exists it.positions.push_back(joint_command_interface_[0][index].get().get_value()); From 5002208d127532aaadfd7c347e58cd5f076b677c Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Tue, 3 May 2022 08:44:27 +0200 Subject: [PATCH 087/524] Fix wrong integration of velocity feedback in odometry in diff_drive_controller (#331) --- .../src/diff_drive_controller.cpp | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e1f0e432df..1ded2e472d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -140,7 +140,7 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons } controller_interface::return_type DiffDriveController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); if (get_state().id() == State::PRIMARY_STATE_INACTIVE) @@ -153,8 +153,6 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - const auto current_time = time; - std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); @@ -164,7 +162,7 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::ERROR; } - const auto age_of_last_command = current_time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { @@ -178,6 +176,8 @@ controller_interface::return_type DiffDriveController::update( double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; + previous_update_timestamp_ = time; + // Apply (possibly new) multipliers: const auto wheels = wheel_params_; const double wheel_separation = wheels.separation_multiplier * wheels.separation; @@ -186,7 +186,7 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.open_loop) { - odometry_.updateOpenLoop(linear_command, angular_command, current_time); + odometry_.updateOpenLoop(linear_command, angular_command, time); } else { @@ -214,25 +214,27 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.position_feedback) { - odometry_.update(left_feedback_mean, right_feedback_mean, current_time); + odometry_.update(left_feedback_mean, right_feedback_mean, time); } else { - odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time); + odometry_.updateFromVelocity( + left_feedback_mean*period.seconds(), right_feedback_mean*period.seconds(), + time); } } tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < current_time) + if (previous_publish_timestamp_ + publish_period_ < time) { previous_publish_timestamp_ += publish_period_; if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = current_time; + odometry_message.header.stamp = time; odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); odometry_message.pose.pose.orientation.x = orientation.x(); @@ -247,7 +249,7 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = current_time; + transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); transform.transform.rotation.x = orientation.x(); @@ -258,15 +260,12 @@ controller_interface::return_type DiffDriveController::update( } } - const auto update_dt = current_time - previous_update_timestamp_; - previous_update_timestamp_ = current_time; - auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; limiter_linear_.limit( - linear_command, last_command.linear.x, second_to_last_command.linear.x, update_dt.seconds()); + linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); limiter_angular_.limit( - angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); + angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); previous_commands_.pop(); previous_commands_.emplace(command); @@ -275,7 +274,7 @@ controller_interface::return_type DiffDriveController::update( if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; - limited_velocity_command.header.stamp = current_time; + limited_velocity_command.header.stamp = time; limited_velocity_command.twist = command.twist; realtime_limited_velocity_publisher_->unlockAndPublish(); } From 7bd93cc9f38d9342b1e0ca3c63bcf1d25e7c6ee4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 May 2022 08:46:21 +0200 Subject: [PATCH 088/524] Create mergify.yml (#307) --- .github/mergify.yml | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 .github/mergify.yml diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000000..6bfd4470ac --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,26 @@ +pull_request_rules: + - name: Backport to galactic at reviewers discretion + conditions: + - base=master + - "label=backport-galactic" + actions: + backport: + branches: + - galactic + + - name: Backport to foxy at reviewers discretion + conditions: + - base=master + - "label=backport-foxy" + actions: + backport: + branches: + - foxy + + - name: Ask to resolve conflict + conditions: + - conflict + - author!=mergify + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? From 8bbccf27c1e936a8012b067818a2a0f909c40605 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Tue, 3 May 2022 10:18:58 -0500 Subject: [PATCH 089/524] fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (#327) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher loop to be ready before finishing configure * test: :white_check_mark: check for published message in test_force_torque_sensor_broadcaster - create a loop that checks if force_torque_sensor_broadcaster published before checking subscribed message * refactor: :recycle: remove unnecessary includes * test: :white_check_mark: port realtime_publisher check to imu_sensor_broadcaster and joint_state_broadcaster * refactor: :recycle: remove unnecessary wait_for code from tests Co-authored-by: Denis Štogl --- .../test_force_torque_sensor_broadcaster.cpp | 30 +++++------ .../test/test_imu_sensor_broadcaster.cpp | 28 +++++----- .../test/test_joint_state_broadcaster.cpp | 52 ++++++++++++------- 3 files changed, 62 insertions(+), 48 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 74c636c596..74e2b15da0 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 @@ -37,15 +37,6 @@ namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(20); - return wait_set.wait(timeout).kind(); -} - } // namespace void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -86,12 +77,21 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); // call update to publish the test value - ASSERT_EQ( - fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; // take message from subscription rclcpp::MessageInfo msg_info; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 1ddc6779ce..42b53bf975 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -38,13 +38,6 @@ namespace constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - return wait_set.wait().kind(); -} - } // namespace void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -88,12 +81,21 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & "/test_imu_sensor_broadcaster/imu", 10, subs_callback); // call update to publish the test value - ASSERT_EQ( - imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; // take message from subscription rclcpp::MessageInfo msg_info; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index f4fe654f5b..ecccaa4b22 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -45,14 +45,6 @@ namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} } // namespace void JointStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -606,12 +598,22 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; // take message from subscription sensor_msgs::msg::JointState joint_state_msg; @@ -658,12 +660,22 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; // take message from subscription control_msgs::msg::DynamicJointState dynamic_joint_state_msg; From ae0706661c3a01a851481457910eda71c1ef5f90 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Wed, 11 May 2022 03:15:04 -0400 Subject: [PATCH 090/524] [diff_drive_controller] Made odom topic name relative as it was in ROS1. (#343) --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1ded2e472d..9460849a75 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -33,7 +33,7 @@ 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_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; } // namespace From f0c8e035ad51b9db732edbad823b710f18ae481c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 13 May 2022 18:03:09 +0100 Subject: [PATCH 091/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 12 files changed, 47 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8cc9002d9b..1569e200ef 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [diff_drive_controller] Made odom topic name relative as it was in ROS1. (`#343 `_) +* Fix wrong integration of velocity feedback in odometry in diff_drive_controller (`#331 `_) +* Contributors: Patrick Roncagliolo, Tony Baltovski + 2.4.0 (2022-04-29) ------------------ * updated to use node getter functions (`#329 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ed9dcf94f0..62924ab273 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ * Multi-interface Forward Controller (`#154 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b2d91e79d1..192655c2d8 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) +* Contributors: Jaron Lundwall, Denis Štogl + 2.4.0 (2022-04-29) ------------------ * updated to use node getter functions (`#329 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 367d24313d..1d765d4f8c 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ * Multi-interface Forward Controller (`#154 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ed6a931618..c9822f4ba5 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ * updated to use node getter functions (`#329 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3a1cde9389..3a0b70c5c1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) +* Contributors: Jaron Lundwall, Denis Štogl + 2.4.0 (2022-04-29) ------------------ * updated to use node getter functions (`#329 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ca8e5ab151..c2c547fed2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) +* Contributors: Jaron Lundwall, Denis Štogl + 2.4.0 (2022-04-29) ------------------ * updated to use node getter functions (`#329 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2d8d3ae6f6..a61b2a656d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* check for nans in command interface (`#346 `_) +* Contributors: Michael Wiznitzer + 2.4.0 (2022-04-29) ------------------ * Fix a gtest deprecation warning (`#341 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e2f56c4429..95643b7afa 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ * Multi-interface Forward Controller (`#154 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 69f0341400..1bfc310aa0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index abb3886655..34a497804b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3fd3fa9879..6f8b024dd1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.0 (2022-04-29) ------------------ * Multi-interface Forward Controller (`#154 `_) From 51c982ac05cd3caed3e6738eb062cf0efc9e0037 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 13 May 2022 18:03:56 +0100 Subject: [PATCH 092/524] 2.5.0 --- 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 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 24 files changed, 36 insertions(+), 36 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1569e200ef..55cc87e272 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ * [diff_drive_controller] Made odom topic name relative as it was in ROS1. (`#343 `_) * Fix wrong integration of velocity feedback in odometry in diff_drive_controller (`#331 `_) * Contributors: Patrick Roncagliolo, Tony Baltovski diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 140ece7490..cac5cbacd5 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.4.0 + 2.5.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 62924ab273..c6151b211a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e4e02c06cd..ec76c7bb59 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.4.0 + 2.5.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 192655c2d8..2977e48e2f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) * Contributors: Jaron Lundwall, Denis Štogl diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f9b7e1893a..f471070c0d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.4.0 + 2.5.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 1d765d4f8c..631adf34d0 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index af1d261235..1cbe639745 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.4.0 + 2.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c9822f4ba5..df0625eed7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 8466acc195..aa781b9d52 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.4.0 + 2.5.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3a0b70c5c1..1fb32f7000 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) * Contributors: Jaron Lundwall, Denis Štogl diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 50db89e5a2..822859ca15 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.4.0 + 2.5.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 c2c547fed2..e199f9f4ea 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) * Contributors: Jaron Lundwall, Denis Štogl diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 4fd6178f9c..bd93fc9158 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.4.0 + 2.5.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 a61b2a656d..2602e4f8c5 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ * check for nans in command interface (`#346 `_) * Contributors: Michael Wiznitzer diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 1c13b114af..0a23072f8c 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.4.0 + 2.5.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 95643b7afa..9af6103e44 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 63e258f0e7..194cdd4272 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.4.0 + 2.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1bfc310aa0..4247596d49 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 9deb6a5e49..742301308a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.4.0 + 2.5.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 34a497804b..a13edb6abc 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 011e50b5c5..9c64cacc66 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.4.0 + 2.5.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6f8b024dd1..7e397925cd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.5.0 (2022-05-13) +------------------ 2.4.0 (2022-04-29) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ea7c332610..50c240458c 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.4.0 + 2.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From ddcee08d95f55bee8738d550aa8707ff5603e67f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 8 Jun 2022 02:02:25 -0500 Subject: [PATCH 093/524] Member variable renaming in the Joint Traj Controller (#361) --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 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 29789b30d3..ccf052e4a6 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 @@ -150,7 +150,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter = false; + bool use_closed_loop_pid_adapter_ = false; using PidPtr = std::shared_ptr; std::vector pids_; /// Feed-forward velocity weight factor when calculating closed loop pid adapter's command diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index eecbecbb04..0d09301f8f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -231,7 +231,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if tolerance is met if (!abort && within_goal_time) { - if (use_closed_loop_pid_adapter) + if (use_closed_loop_pid_adapter_) { // Update PIDs for (auto i = 0ul; i < joint_num; ++i) @@ -251,7 +251,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - if (use_closed_loop_pid_adapter) + if (use_closed_loop_pid_adapter_) { assign_interface_from_point(joint_command_interface_[1], tmp_command_); } @@ -266,7 +266,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter) + if (use_closed_loop_pid_adapter_) { assign_interface_from_point(joint_command_interface_[3], tmp_command_); } @@ -513,7 +513,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // if there is only velocity then use also PID adapter if (command_interface_types_.size() == 1) { - use_closed_loop_pid_adapter = true; + use_closed_loop_pid_adapter_ = true; } else if (!has_position_command_interface_) { @@ -539,7 +539,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { if (command_interface_types_.size() == 1) { - use_closed_loop_pid_adapter = true; + use_closed_loop_pid_adapter_ = true; } else { @@ -548,7 +548,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } - if (use_closed_loop_pid_adapter) + if (use_closed_loop_pid_adapter_) { size_t num_joints = joint_names_.size(); pids_.resize(num_joints); From 547cef784df4fda35cebd6a1c1ab282d7b24f83d Mon Sep 17 00:00:00 2001 From: Jafar Date: Tue, 14 Jun 2022 12:42:46 +0300 Subject: [PATCH 094/524] Fix exception about parameter already been declared & Change default c++ version to 17 (#360) * Default C++ version to 17 * Replace explicit use of declare_paremeter with auto_declare --- diff_drive_controller/CMakeLists.txt | 5 +++-- effort_controllers/CMakeLists.txt | 5 +++-- force_torque_sensor_broadcaster/CMakeLists.txt | 5 +++-- forward_command_controller/CMakeLists.txt | 5 +++-- .../src/forward_command_controller.cpp | 4 ++-- .../src/multi_interface_forward_command_controller.cpp | 4 ++-- gripper_controllers/CMakeLists.txt | 5 +++-- imu_sensor_broadcaster/CMakeLists.txt | 5 +++-- joint_state_broadcaster/CMakeLists.txt | 5 +++-- joint_trajectory_controller/CMakeLists.txt | 5 +++-- position_controllers/CMakeLists.txt | 5 +++-- velocity_controllers/CMakeLists.txt | 5 +++-- 12 files changed, 34 insertions(+), 24 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 0db84ec8e9..5fe5766982 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(diff_drive_controller) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index dc13f1fd4e..886d1286cc 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(effort_controllers) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index d5b3f792f2..0bffc7b52d 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(force_torque_sensor_broadcaster) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 2490b5c856..e0cb68ebea 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(forward_command_controller) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 46e90cd1c4..06c6b15120 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -29,8 +29,8 @@ ForwardCommandController::ForwardCommandController() : ForwardControllersBase() void ForwardCommandController::declare_parameters() { - get_node()->declare_parameter>("joints", std::vector()); - get_node()->declare_parameter("interface_name", ""); + auto_declare("joints", std::vector()); + auto_declare("interface_name", std::string()); } controller_interface::CallbackReturn ForwardCommandController::read_parameters() diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 6cc22cf549..8faacd0776 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -26,8 +26,8 @@ MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() void MultiInterfaceForwardCommandController::declare_parameters() { - get_node()->declare_parameter("joint", joint_name_); - get_node()->declare_parameter>("interface_names", interface_names_); + auto_declare("joint", joint_name_); + auto_declare("interface_names", interface_names_); } controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 07a472dcef..3c5d933148 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -6,9 +6,10 @@ if(APPLE OR WIN32) return() endif() -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 2a530ab582..d39c5c67be 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(imu_sensor_broadcaster) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 2848617db2..f8c14de1ef 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(joint_state_broadcaster) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5a1836573f..9495bcc0dd 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(joint_trajectory_controller) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index be010fd1d7..9acc8e360b 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(position_controllers) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 1635ace140..a3ae87dd34 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -1,9 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(velocity_controllers) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From 801b708f58d0e63fab635d03f6bf4fc36e512a02 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 14 Jun 2022 04:45:29 -0500 Subject: [PATCH 095/524] CMakeLists cleanup (#362) --- diff_drive_controller/CMakeLists.txt | 66 +++++++-------- effort_controllers/CMakeLists.txt | 31 +++---- .../CMakeLists.txt | 56 ++++++------- forward_command_controller/CMakeLists.txt | 55 +++++-------- gripper_controllers/CMakeLists.txt | 37 ++++----- imu_sensor_broadcaster/CMakeLists.txt | 51 +++++------- joint_trajectory_controller/CMakeLists.txt | 81 +++++++------------ position_controllers/CMakeLists.txt | 31 +++---- velocity_controllers/CMakeLists.txt | 27 ++++--- 9 files changed, 187 insertions(+), 248 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 5fe5766982..2b6de94e0b 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -11,50 +11,44 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rcpputils REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(diff_drive_controller SHARED +add_library(${PROJECT_NAME} SHARED src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp ) - -target_include_directories(diff_drive_controller PRIVATE include) -ament_target_dependencies(diff_drive_controller - builtin_interfaces - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs -) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) install(DIRECTORY include/ DESTINATION include ) -install(TARGETS diff_drive_controller +install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -70,7 +64,7 @@ if(BUILD_TESTING) ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) target_include_directories(test_diff_drive_controller PRIVATE include) target_link_libraries(test_diff_drive_controller - diff_drive_controller + ${PROJECT_NAME} ) ament_target_dependencies(test_diff_drive_controller @@ -97,18 +91,12 @@ if(BUILD_TESTING) endif() ament_export_dependencies( - controller_interface - geometry_msgs - hardware_interface - rclcpp - rclcpp_lifecycle - tf2 - tf2_msgs + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_include_directories( include ) ament_export_libraries( - diff_drive_controller + ${PROJECT_NAME} ) ament_package() diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 886d1286cc..91632606bf 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -11,25 +11,28 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + forward_command_controller + pluginlib + rclcpp +) + find_package(ament_cmake REQUIRED) -find_package(forward_command_controller REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(effort_controllers +add_library(${PROJECT_NAME} SHARED src/joint_group_effort_controller.cpp ) -target_include_directories(effort_controllers PRIVATE include) -ament_target_dependencies(effort_controllers - forward_command_controller - pluginlib - rclcpp +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) install( @@ -39,7 +42,7 @@ install( install( TARGETS - effort_controllers + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -66,7 +69,7 @@ if(BUILD_TESTING) ) target_include_directories(test_joint_group_effort_controller PRIVATE include) target_link_libraries(test_joint_group_effort_controller - effort_controllers + ${PROJECT_NAME} ) endif() @@ -77,6 +80,6 @@ ament_export_include_directories( include ) ament_export_libraries( - effort_controllers + ${PROJECT_NAME} ) ament_package() diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 0bffc7b52d..d68f56958e 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -11,46 +11,43 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools +) + find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() add_library( - force_torque_sensor_broadcaster + ${PROJECT_NAME} SHARED src/force_torque_sensor_broadcaster.cpp ) -target_include_directories( - force_torque_sensor_broadcaster - PUBLIC - include -) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $) ament_target_dependencies( - force_torque_sensor_broadcaster - geometry_msgs - controller_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools + ${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) install( TARGETS - force_torque_sensor_broadcaster + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -84,7 +81,7 @@ if(BUILD_TESTING) ) target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster - force_torque_sensor_broadcaster + ${PROJECT_NAME} ) ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface @@ -100,15 +97,10 @@ ament_export_include_directories( include ) ament_export_libraries( - force_torque_sensor_broadcaster + ${PROJECT_NAME} ) ament_export_dependencies( - controller_interface - pluginlib - rclcpp - rclcpp_lifecycle - geometry_msgs - hardware_interface + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index e0cb68ebea..f1d7a18328 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -11,36 +11,32 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs +) + find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(std_msgs REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(forward_command_controller +add_library(${PROJECT_NAME} SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(forward_command_controller PRIVATE include) -ament_target_dependencies(forward_command_controller - builtin_interfaces - controller_interface - hardware_interface - pluginlib - rclcpp_lifecycle - rcutils - realtime_tools - std_msgs -) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) install( @@ -50,7 +46,7 @@ install( install( TARGETS - forward_command_controller + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -79,7 +75,7 @@ if(BUILD_TESTING) ) target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller - forward_command_controller + ${PROJECT_NAME} ) ament_add_gmock( @@ -100,22 +96,15 @@ if(BUILD_TESTING) ) target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller - forward_command_controller + ${PROJECT_NAME} ) endif() -ament_export_dependencies( - controller_interface - hardware_interface - rclcpp - rclcpp_lifecycle - realtime_tools - std_msgs -) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_export_include_directories( include ) ament_export_libraries( - forward_command_controller + ${PROJECT_NAME} ) ament_package() diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 3c5d933148..40ca2fdd32 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -16,30 +16,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp_action + control_msgs + control_toolbox + controller_interface + hardware_interface + pluginlib + realtime_tools + rclcpp +) + find_package(ament_cmake REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(control_msgs REQUIRED) -find_package(control_toolbox REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(rclcpp REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) -ament_target_dependencies(gripper_action_controller - controller_interface - control_msgs - control_toolbox - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - rcutils - realtime_tools -) +ament_target_dependencies(gripper_action_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_include_directories(gripper_action_controller PRIVATE $ PRIVATE $ @@ -49,17 +45,14 @@ pluginlib_export_plugin_description_file(controller_interface ros_control_plugin ## Install ## ############# -# Install headers install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include ) -# Install targets install(TARGETS gripper_action_controller LIBRARY DESTINATION lib ) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index d39c5c67be..5febe1c048 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -11,46 +11,42 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + sensor_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools +) + find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() add_library( - imu_sensor_broadcaster + ${PROJECT_NAME} SHARED src/imu_sensor_broadcaster.cpp ) target_include_directories( - imu_sensor_broadcaster + ${PROJECT_NAME} PUBLIC include ) -ament_target_dependencies( - imu_sensor_broadcaster - sensor_msgs - controller_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools -) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) install( TARGETS - imu_sensor_broadcaster + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -84,7 +80,7 @@ if(BUILD_TESTING) ) target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster - imu_sensor_broadcaster + ${PROJECT_NAME} ) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface @@ -100,15 +96,10 @@ ament_export_include_directories( include ) ament_export_libraries( - imu_sensor_broadcaster + ${PROJECT_NAME} ) ament_export_dependencies( - controller_interface - pluginlib - rclcpp - rclcpp_lifecycle - geometry_msgs - hardware_interface + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 9495bcc0dd..5636465711 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -11,46 +11,40 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + controller_interface + control_msgs + control_toolbox + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + trajectory_msgs +) + find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(controller_interface REQUIRED) -find_package(control_msgs REQUIRED) -find_package(control_toolbox REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(trajectory_msgs REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(joint_trajectory_controller SHARED +add_library(${PROJECT_NAME} SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) -target_include_directories(joint_trajectory_controller PRIVATE include) -ament_target_dependencies(joint_trajectory_controller - angles - builtin_interfaces - controller_interface - control_msgs - control_toolbox - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - trajectory_msgs -) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) install(DIRECTORY include/ DESTINATION include ) -install(TARGETS joint_trajectory_controller +install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -63,7 +57,7 @@ if(BUILD_TESTING) ament_add_gtest(test_trajectory test/test_trajectory.cpp) target_include_directories(test_trajectory PRIVATE include) - target_link_libraries(test_trajectory joint_trajectory_controller) + target_link_libraries(test_trajectory ${PROJECT_NAME}) ament_add_gtest(test_trajectory_controller test/test_trajectory_controller.cpp @@ -71,17 +65,10 @@ if(BUILD_TESTING) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller - joint_trajectory_controller + ${PROJECT_NAME} ) ament_target_dependencies(test_trajectory_controller - control_msgs - hardware_interface - rclcpp - rclcpp_lifecycle - control_toolbox - realtime_tools - ros2_control_test_assets - trajectory_msgs + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_add_gtest( @@ -102,32 +89,20 @@ if(BUILD_TESTING) ) target_include_directories(test_trajectory_actions PRIVATE include) target_link_libraries(test_trajectory_actions - joint_trajectory_controller + ${PROJECT_NAME} ) ament_target_dependencies(test_trajectory_actions - control_msgs - hardware_interface - rclcpp - rclcpp_lifecycle - ros2_control_test_assets - trajectory_msgs - realtime_tools + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) endif() ament_export_dependencies( - controller_interface - control_msgs - control_toolbox - hardware_interface - rclcpp - rclcpp_lifecycle - trajectory_msgs + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_include_directories( include ) ament_export_libraries( - joint_trajectory_controller + ${PROJECT_NAME} ) ament_package() diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 9acc8e360b..f1f57c6180 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -11,25 +11,28 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + forward_command_controller + pluginlib + rclcpp +) + find_package(ament_cmake REQUIRED) -find_package(forward_command_controller REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(position_controllers +add_library(${PROJECT_NAME} SHARED src/joint_group_position_controller.cpp ) -target_include_directories(position_controllers PRIVATE include) -ament_target_dependencies(position_controllers - forward_command_controller - pluginlib - rclcpp +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) install( @@ -39,7 +42,7 @@ install( install( TARGETS - position_controllers + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -66,7 +69,7 @@ if(BUILD_TESTING) ) target_include_directories(test_joint_group_position_controller PRIVATE include) target_link_libraries(test_joint_group_position_controller - position_controllers + ${PROJECT_NAME} ) endif() @@ -77,6 +80,6 @@ ament_export_include_directories( include ) ament_export_libraries( - position_controllers + ${PROJECT_NAME} ) ament_package() diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index a3ae87dd34..5f0560659a 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -11,25 +11,30 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + forward_command_controller + pluginlib + rclcpp +) + find_package(ament_cmake REQUIRED) -find_package(forward_command_controller REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library(velocity_controllers +add_library(${PROJECT_NAME} SHARED src/joint_group_velocity_controller.cpp ) -target_include_directories(velocity_controllers PRIVATE include) -ament_target_dependencies(velocity_controllers +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} forward_command_controller pluginlib rclcpp ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) @@ -40,7 +45,7 @@ install( install( TARGETS - velocity_controllers + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -67,7 +72,7 @@ if(BUILD_TESTING) ) target_include_directories(test_joint_group_velocity_controller PRIVATE include) target_link_libraries(test_joint_group_velocity_controller - velocity_controllers + ${PROJECT_NAME} ) endif() @@ -78,6 +83,6 @@ ament_export_include_directories( include ) ament_export_libraries( - velocity_controllers + ${PROJECT_NAME} ) ament_package() From 679fc0f938c3ad56366e321dd3ac64144a1febc0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 16 Jun 2022 22:49:09 +0100 Subject: [PATCH 096/524] Fixed lof message in joint_trayectory_controller (#366) Signed-off-by: ahcorde --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0d09301f8f..1d98b72ad4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -663,7 +663,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Print output so users can be sure the interface setup is correct RCLCPP_INFO( - logger, "Command interfaces are [%s] and and state interfaces are [%s].", + logger, "Command interfaces are [%s] and state interfaces are [%s].", get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); From bcf7eb344b326acc79d2592294ff426673dd90b1 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 16 Jun 2022 16:54:09 -0500 Subject: [PATCH 097/524] Disable failing workflows (#363) --- .github/workflows/ci-coverage-build.yml | 55 ------------------- .github/workflows/foxy-abi-compatibility.yml | 21 +++---- .../workflows/galactic-abi-compatibility.yml | 21 +++---- .../reusable-ros-tooling-source-build.yml | 2 +- .../rolling-abi-compatibility-last-focal.yml | 21 ------- .../rolling-binary-build-last-focal.yml | 26 --------- .../workflows/rolling-rhel-binary-build.yml | 33 +++++------ .../rolling-semi-binary-build-last-focal.yml | 26 --------- diff_drive_controller/CMakeLists.txt | 4 +- .../CMakeLists.txt | 2 +- joint_trajectory_controller/CMakeLists.txt | 23 ++++---- .../test/test_trajectory_actions.cpp | 1 - .../test/test_trajectory_controller.cpp | 3 + 13 files changed, 58 insertions(+), 180 deletions(-) delete mode 100644 .github/workflows/ci-coverage-build.yml delete mode 100644 .github/workflows/rolling-abi-compatibility-last-focal.yml delete mode 100644 .github/workflows/rolling-binary-build-last-focal.yml delete mode 100644 .github/workflows/rolling-semi-binary-build-last-focal.yml diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml deleted file mode 100644 index 8176bbdf5c..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,55 +0,0 @@ -name: Coverage Build -on: - pull_request: - branches: - - master - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-20.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: rolling - steps: - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@v0.2 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed in the meta package - package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - 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 }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - skip-tests: true - - uses: codecov/codecov-action@v1.0.14 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v1 - with: - name: colcon-logs - path: ros_ws/log diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml index 40b1e1133f..7f9f1684ef 100644 --- a/.github/workflows/foxy-abi-compatibility.yml +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -5,13 +5,14 @@ on: - foxy jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: foxy - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true +# TODO(andyz): disabled because it started failing at the transition to Humble +# abi_check: +# runs-on: ubuntu-latest +# steps: +# - uses: actions/checkout@v3 +# - uses: ros-industrial/industrial_ci@master +# env: +# ROS_DISTRO: foxy +# ROS_REPO: main +# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} +# NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml index 41c663dd9c..8689791128 100644 --- a/.github/workflows/galactic-abi-compatibility.yml +++ b/.github/workflows/galactic-abi-compatibility.yml @@ -5,13 +5,14 @@ on: - galactic jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: galactic - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true +# TODO(andyz): disabled because it started failing at the transition to Humble +# abi_check: +# runs-on: ubuntu-latest +# steps: +# - uses: actions/checkout@v3 +# - uses: ros-industrial/industrial_ci@master +# env: +# ROS_DISTRO: galactic +# ROS_REPO: main +# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} +# NOT_TEST_BUILD: true diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f919573a86..287809991d 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -44,7 +44,7 @@ jobs: ros2_controllers_test_nodes velocity_controllers vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros_distro }}/ros2.repos + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ref }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - uses: actions/upload-artifact@v1 diff --git a/.github/workflows/rolling-abi-compatibility-last-focal.yml b/.github/workflows/rolling-abi-compatibility-last-focal.yml deleted file mode 100644 index 9ad07243d9..0000000000 --- a/.github/workflows/rolling-abi-compatibility-last-focal.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: ABI Compatibility Check - last Focal -on: - pull_request: - branches: - - master - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: rolling - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true - OS_CODE_NAME: focal - BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/.github/workflows/rolling-binary-build-last-focal.yml b/.github/workflows/rolling-binary-build-last-focal.yml deleted file mode 100644 index 41a8980780..0000000000 --- a/.github/workflows/rolling-binary-build-last-focal.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Rolling Binary Build - Last Focal -# Build & test all dependencies from released (binary) packages. - -on: - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - upstream_workspace: ros2_controllers-not-released.rolling.repos - ref_for_scheduled_build: master - ros_repo: main - os_code_name: focal - before_install_upstream_dependencies: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index cd35303f3f..62197a2db9 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -12,19 +12,20 @@ on: jobs: - rolling_rhel_binary: - name: Rolling RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: jaronl/ros:rolling-alma - steps: - - uses: actions/checkout@v3 - 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 +# TODO(andyz): disabled because it started failing at the transition to Humble +# rolling_rhel_binary: +# name: Rolling RHEL binary build +# runs-on: ubuntu-latest +# env: +# ROS_DISTRO: rolling +# container: jaronl/ros:rolling-alma +# steps: +# - uses: actions/checkout@v3 +# 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/rolling-semi-binary-build-last-focal.yml b/.github/workflows/rolling-semi-binary-build-last-focal.yml deleted file mode 100644 index e4f6d4c91c..0000000000 --- a/.github/workflows/rolling-semi-binary-build-last-focal.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Rolling Semi-Binary Build - Last Focal -# Build & test that compiles the main dependencies from source. - -on: - pull_request: - branches: - - master - push: - branches: - - master - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: rolling - upstream_workspace: ros2_controllers.rolling.repos - ref_for_scheduled_build: master - ros_repo: main - os_code_name: focal - before_install_upstream_dependencies: | - export ROSDISTRO_INDEX_URL=https://raw.githubusercontent.com/ros/rosdistro/rolling/2022-01-28/index-v4.yaml - rosdep update diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 2b6de94e0b..09ca27e1d2 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -36,8 +36,8 @@ add_library(${PROJECT_NAME} SHARED src/speed_limiter.cpp ) target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) + PUBLIC $ + $) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index d68f56958e..a710586700 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -33,7 +33,7 @@ add_library( ) target_include_directories(${PROJECT_NAME} PUBLIC $ - $) + $) ament_target_dependencies( ${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5636465711..793b736a05 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -83,17 +83,18 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gtest( - test_trajectory_actions - test/test_trajectory_actions.cpp - ) - target_include_directories(test_trajectory_actions PRIVATE include) - target_link_libraries(test_trajectory_actions - ${PROJECT_NAME} - ) - ament_target_dependencies(test_trajectory_actions - ${THIS_PACKAGE_INCLUDE_DEPENDS} - ) + # TODO(andyz): Disabled due to flakiness + # ament_add_gtest( + # test_trajectory_actions + # test/test_trajectory_actions.cpp + # ) + # target_include_directories(test_trajectory_actions PRIVATE include) + # target_link_libraries(test_trajectory_actions + # ${PROJECT_NAME} + # ) + # ament_target_dependencies(test_trajectory_actions + # ${THIS_PACKAGE_INCLUDE_DEPENDS} + # ) endif() ament_export_dependencies( diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fe65711aef..1b0dc25c3a 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -98,7 +98,6 @@ class TestTrajectoryActions : public TrajectoryControllerTest } }); - // common_goal_response and common_result_response // sometimes doesn't receive calls when we don't sleep std::this_thread::sleep_for(std::chrono::milliseconds(300)); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a4b429bdc0..57b0a36251 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1229,6 +1229,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co executor.cancel(); } +// TODO(andyz): disabled because they started failing at the transition to Humble +/* // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, @@ -1286,6 +1288,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); +*/ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { From 28b430ebfe89d37897e299881acdef762fde64fb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 18 Jun 2022 07:56:46 +0100 Subject: [PATCH 098/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 9 +++++++++ effort_controllers/CHANGELOG.rst | 8 ++++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 9 +++++++++ forward_command_controller/CHANGELOG.rst | 8 ++++++++ gripper_controllers/CHANGELOG.rst | 8 ++++++++ imu_sensor_broadcaster/CHANGELOG.rst | 8 ++++++++ joint_state_broadcaster/CHANGELOG.rst | 7 +++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 8 ++++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 8 ++++++++ 12 files changed, 90 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 55cc87e272..4267e6da29 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Disable failing workflows (`#363 `_) +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ * [diff_drive_controller] Made odom topic name relative as it was in ROS1. (`#343 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c6151b211a..507fde37b4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 2977e48e2f..4fa94ff164 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Disable failing workflows (`#363 `_) +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 631adf34d0..f3a12793b0 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index df0625eed7..d70ee67f37 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1fb32f7000..c6ec2b04e1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e199f9f4ea..1ed55a0e3f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Jafar Abdi + 2.5.0 (2022-05-13) ------------------ * fix: :bug: make force_torque_sensor_broadcaster wait for realtime_publisher (`#327 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2602e4f8c5..32fb4ca24a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Disable failing workflows (`#363 `_) +* Fixed lof message in joint_trayectory_controller (`#366 `_) +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Member variable renaming in the Joint Traj Controller (`#361 `_) +* Contributors: Alejandro Hernández Cordero, Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ * check for nans in command interface (`#346 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 9af6103e44..14a268d558 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + 2.5.0 (2022-05-13) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4247596d49..44e7d51b26 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2022-05-13) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a13edb6abc..23a4bdc82f 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.5.0 (2022-05-13) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 7e397925cd..93a67c1c0d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: AndyZe, Jafar + 2.5.0 (2022-05-13) ------------------ From 323295270bb9417e5d200f18c86f2c6327851fee Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 18 Jun 2022 07:57:03 +0100 Subject: [PATCH 099/524] 2.6.0 --- 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 +- 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 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 25 files changed, 37 insertions(+), 37 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4267e6da29..490315a972 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * Disable failing workflows (`#363 `_) * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index cac5cbacd5..5e2e0f9e41 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.5.0 + 2.6.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 507fde37b4..6cfa88eb1f 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ec76c7bb59..78cac630ae 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.5.0 + 2.6.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 4fa94ff164..3c017d6d80 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * Disable failing workflows (`#363 `_) * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f471070c0d..ae6e966e2e 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.5.0 + 2.6.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 f3a12793b0..fa9e635f77 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1cbe639745..75d5c90f68 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.5.0 + 2.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d70ee67f37..5d8424339d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index aa781b9d52..499ed57d94 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.5.0 + 2.6.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c6ec2b04e1..9ee1fcdd10 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 822859ca15..2064a7aea7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.5.0 + 2.6.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 1ed55a0e3f..5d561fa127 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 * Replace explicit use of declare_paremeter with auto_declare diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index bd93fc9158..3903c753da 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.5.0 + 2.6.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 32fb4ca24a..84b7d32d78 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * Disable failing workflows (`#363 `_) * Fixed lof message in joint_trayectory_controller (`#366 `_) * CMakeLists cleanup (`#362 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 0a23072f8c..79236cb624 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.5.0 + 2.6.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 14a268d558..ed3a204dc1 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 194cdd4272..bf995e45e9 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.5.0 + 2.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 44e7d51b26..50ed6580da 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ 2.5.0 (2022-05-13) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 742301308a..7aadfe5fd1 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.5.0 + 2.6.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 23a4bdc82f..c11afbd1f1 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ 2.5.0 (2022-05-13) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 9c64cacc66..854057cbe9 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.5.0 + 2.6.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 061e7aef38..47511d03b3 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="0.0.1", + version="2.6.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 93a67c1c0d..4dc31af927 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.6.0 (2022-06-18) +------------------ * CMakeLists cleanup (`#362 `_) * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) * Default C++ version to 17 diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 50c240458c..2ecf32b0f6 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.5.0 + 2.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From d5507c3752ac8d5f16f9c96a9fe7be56cafd3535 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Mon, 20 Jun 2022 02:54:11 -0400 Subject: [PATCH 100/524] account for edge case in JTC (#350) Co-authored-by: Michael Wiznitzer --- .../src/joint_trajectory_controller.cpp | 6 ++- .../test/test_trajectory_actions.cpp | 47 +++++++++++++++++++ 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1d98b72ad4..8e34e541ed 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -165,9 +165,11 @@ controller_interface::return_type JointTrajectoryController::update( // currently carrying out a trajectory if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { + bool first_sample = false; // if sampling the first time, set the point before you sample if (!(*traj_point_active_ptr_)->is_sampled_already()) { + first_sample = true; if (open_loop_control_) { (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); @@ -197,8 +199,10 @@ controller_interface::return_type JointTrajectoryController::update( { compute_error_for_joint(state_error, index, state_current, state_desired); + // Always check the state tolerance on the first sample in case the first sample + // is the last point if ( - before_last_point && + (before_last_point || first_sample) && !check_state_tolerance_per_joint( state_error, index, default_tolerances_.state_tolerance[index], false)) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 1b0dc25c3a..cc91071e0c 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -471,6 +471,53 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); } +TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + SetUpExecutor(params); + SetUpControllerHardware(); + + const double init_pos1 = joint_pos_[0]; + const double init_pos2 = joint_pos_[1]; + const double init_pos3 = joint_pos_[2]; + + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 4.0; + point.positions[1] = 5.0; + point.positions[2] = 6.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, + common_action_result_code_); + + // run an update, it should be holding + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); +} + TEST_F(TestTrajectoryActions, test_cancel_hold_position) { SetUpExecutor(); From ea6123abc1d91b270351372251eadbf58015e6f8 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 27 Jun 2022 05:25:49 -0500 Subject: [PATCH 101/524] Rename the "abort" variable in the joint traj controller (#367) --- .../src/joint_trajectory_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 8e34e541ed..e1cbecc103 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -188,7 +188,7 @@ controller_interface::return_type JointTrajectoryController::update( if (valid_point) { - bool abort = false; + bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; double time_difference = 0.0; @@ -206,7 +206,7 @@ controller_interface::return_type JointTrajectoryController::update( !check_state_tolerance_per_joint( state_error, index, default_tolerances_.state_tolerance[index], false)) { - abort = true; + tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( @@ -233,7 +233,7 @@ controller_interface::return_type JointTrajectoryController::update( } // set values for next hardware write() if tolerance is met - if (!abort && within_goal_time) + if (!tolerance_violated_while_moving && within_goal_time) { if (use_closed_loop_pid_adapter_) { @@ -298,7 +298,7 @@ controller_interface::return_type JointTrajectoryController::update( active_goal->setFeedback(feedback); // check abort - if (abort) + if (tolerance_violated_while_moving) { set_hold_position(); auto result = std::make_shared(); From 49e64937578b8cbf2d22782b2864cfd7aab7b802 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 28 Jun 2022 12:10:14 +0100 Subject: [PATCH 102/524] Update reviewers from ros2_control --- .github/reviewer-lottery.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index fbaf954340..a5a67c6b56 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -24,7 +24,6 @@ groups: - jaron-l - malapatiravi - erickisos - - ShawnSchaerer - Briancbn - TomoyaFujita2016 - homalozoa @@ -39,3 +38,5 @@ groups: - duringhof - bijoua29 - kasiceo + - lm2292 + - mcbed From 4163f142492c91c5dfb359cb1b38c8c19e659f55 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 29 Jun 2022 13:12:58 -0500 Subject: [PATCH 103/524] Properly retrieve parameters in the Joint Trajectory Controller (#365) --- .../joint_trajectory_controller.hpp | 3 ++ .../src/joint_trajectory_controller.cpp | 41 +++++++++---------- 2 files changed, 22 insertions(+), 22 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 ccf052e4a6..dfb2358475 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 @@ -132,6 +132,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa /// Allow integration in goal trajectories to accept goals without position or velocity specified bool allow_integration_in_goal_trajectories_ = false; + double state_publish_rate_; + double action_monitor_rate_; + // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e1cbecc103..aabc2fbb53 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -55,17 +55,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() try { // with the lifecycle node being initialized, we can declare parameters - auto_declare>("joints", joint_names_); - auto_declare>("command_interfaces", command_interface_types_); - auto_declare>("state_interfaces", state_interface_types_); - auto_declare("state_publish_rate", 50.0); - auto_declare("action_monitor_rate", 20.0); - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - auto_declare("open_loop_control", open_loop_control_); - auto_declare( + joint_names_ = auto_declare>("joints", joint_names_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + allow_partial_joints_goal_ = + auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); + open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); + allow_integration_in_goal_trajectories_ = auto_declare( "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); - auto_declare("constraints.stopped_velocity_tolerance", 0.01); - auto_declare("constraints.goal_time", 0.0); + state_publish_rate_ = auto_declare("state_publish_rate", 50.0); + action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); } catch (const std::exception & e) { @@ -704,12 +705,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = - get_node()->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); - if (state_publish_rate > 0.0) + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); + if (state_publish_rate_ > 0.0) { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate); + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); } else { @@ -751,11 +750,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } - const double action_monitor_rate = - get_node()->get_parameter("action_monitor_rate").get_value(); - - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); + RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( @@ -1068,8 +1064,9 @@ void JointTrajectoryController::fill_partial_goal( // Assume hold position with 0 velocity and acceleration for missing joints if (!it.positions.empty()) { - if (has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) + if ( + has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) { // copy last command if cmd interface exists it.positions.push_back(joint_command_interface_[0][index].get().get_value()); From 6dc1c20c4159b61341813ef5e32b62a805f9a041 Mon Sep 17 00:00:00 2001 From: Schulze Date: Sat, 2 Jul 2022 19:33:15 -0300 Subject: [PATCH 104/524] Update controllers with new get_name hardware interfaces (#369) --- diff_drive_controller/src/diff_drive_controller.cpp | 7 +++---- .../gripper_action_controller_impl.hpp | 12 ++++++------ .../src/joint_state_broadcaster.cpp | 10 +++++----- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 9460849a75..bc2e7eed22 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -219,8 +219,7 @@ controller_interface::return_type DiffDriveController::update( else { odometry_.updateFromVelocity( - left_feedback_mean*period.seconds(), right_feedback_mean*period.seconds(), - time); + left_feedback_mean * period.seconds(), right_feedback_mean * period.seconds(), time); } } @@ -626,7 +625,7 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name, &interface_name](const auto & interface) { - return interface.get_name() == wheel_name && + return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == interface_name; }); @@ -639,7 +638,7 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface) { - return interface.get_name() == wheel_name && + return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == HW_IF_VELOCITY; }); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 2cd66776b5..748887f612 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -232,11 +232,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_name() != joint_name_) + if (position_command_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_name() << "` != `" + << position_command_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -250,11 +250,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_state_interface_it->get_name() != joint_name_) + if (position_state_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_name() << "` != `" + << position_state_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -268,11 +268,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); return controller_interface::CallbackReturn::ERROR; } - if (velocity_state_interface_it->get_name() != joint_name_) + if (velocity_state_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_name() << "` != `" + << velocity_state_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a1ad76b092..65d9ec87c2 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -237,9 +237,9 @@ bool JointStateBroadcaster::init_joint_data() for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { // initialize map if name is new - if (name_if_value_mapping_.count(si->get_name()) == 0) + if (name_if_value_mapping_.count(si->get_prefix_name()) == 0) { - name_if_value_mapping_[si->get_name()] = {}; + name_if_value_mapping_[si->get_prefix_name()] = {}; } // add interface name std::string interface_name = si->get_interface_name(); @@ -247,7 +247,7 @@ bool JointStateBroadcaster::init_joint_data() { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[si->get_name()][interface_name] = kUninitializedValue; + name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; } // filter state interfaces that have at least one of the joint_states fields, @@ -345,10 +345,10 @@ controller_interface::return_type JointStateBroadcaster::update( { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[state_interface.get_name()][interface_name] = + name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s: %f\n", state_interface.get_full_name().c_str(), + get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), state_interface.get_value()); } From 649296a66f36bf8c8559ea90cb547617576ea09a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 3 Jul 2022 17:13:50 +0100 Subject: [PATCH 105/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 12 files changed, 46 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 490315a972..b85c65399e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controllers with new get_name hardware interfaces (`#369 `_) +* Contributors: Lucas Schulze + 2.6.0 (2022-06-18) ------------------ * Disable failing workflows (`#363 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6cfa88eb1f..b0ab26d127 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 3c017d6d80..343af4a18f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * Disable failing workflows (`#363 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index fa9e635f77..443204e70f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5d8424339d..ba964821ec 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controllers with new get_name hardware interfaces (`#369 `_) +* Contributors: Lucas Schulze + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9ee1fcdd10..97307487ed 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5d561fa127..b583c47d38 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update controllers with new get_name hardware interfaces (`#369 `_) +* Contributors: Lucas Schulze + 2.6.0 (2022-06-18) ------------------ * Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 84b7d32d78..29716e6c2e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Properly retrieve parameters in the Joint Trajectory Controller (`#365 `_) +* Rename the "abort" variable in the joint traj controller (`#367 `_) +* account for edge case in JTC (`#350 `_) +* Contributors: Andy Zelenak, Michael Wiznitzer + 2.6.0 (2022-06-18) ------------------ * Disable failing workflows (`#363 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ed3a204dc1..3a5cdea210 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 50ed6580da..472925eda2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c11afbd1f1..8feab14f3e 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 4dc31af927..43e538aa59 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.6.0 (2022-06-18) ------------------ * CMakeLists cleanup (`#362 `_) From 93a9cbe4b4a584b09fa55c57fb38756234709783 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 3 Jul 2022 17:15:05 +0100 Subject: [PATCH 106/524] 2.7.0 --- 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 +- 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 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 25 files changed, 37 insertions(+), 37 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b85c65399e..928627afdd 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) * Contributors: Lucas Schulze diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 5e2e0f9e41..6345e3a042 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.6.0 + 2.7.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 b0ab26d127..ea792565ca 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 78cac630ae..01efb74b7f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.6.0 + 2.7.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 343af4a18f..2b1ca87ab8 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index ae6e966e2e..e2b39122e2 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.6.0 + 2.7.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 443204e70f..91bd17982a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 75d5c90f68..19c0b65d62 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.6.0 + 2.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ba964821ec..8dcf1fd4b3 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) * Contributors: Lucas Schulze diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 499ed57d94..85416605ac 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.6.0 + 2.7.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 97307487ed..2c4ff3ffce 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 2064a7aea7..c7c4acaf10 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.6.0 + 2.7.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 b583c47d38..3730652f4e 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) * Contributors: Lucas Schulze diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 3903c753da..3c2842fd10 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.6.0 + 2.7.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 29716e6c2e..ef8b23875b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ * Properly retrieve parameters in the Joint Trajectory Controller (`#365 `_) * Rename the "abort" variable in the joint traj controller (`#367 `_) * account for edge case in JTC (`#350 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 79236cb624..385ad3d10b 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.6.0 + 2.7.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 3a5cdea210..05c1c8be15 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index bf995e45e9..ce59d22ef2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.6.0 + 2.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 472925eda2..6d2a5cad11 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 7aadfe5fd1..522db24b1c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.6.0 + 2.7.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 8feab14f3e..bf159b7431 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 854057cbe9..0ed743ad9e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.6.0 + 2.7.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 47511d03b3..eb23d114c8 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.6.0", + version="2.7.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 43e538aa59..c6334a85b2 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.7.0 (2022-07-03) +------------------ 2.6.0 (2022-06-18) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 2ecf32b0f6..7ad6052d35 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.6.0 + 2.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 9683666a7f67835b95da9593bf85b3f62517e841 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 4 Jul 2022 13:47:31 +0200 Subject: [PATCH 107/524] Fix deprecation in setup.cfg on Jammy (Humble and Rolling). (#375) --- ros2_controllers_test_nodes/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_controllers_test_nodes/setup.cfg b/ros2_controllers_test_nodes/setup.cfg index c00c975bcf..ec192fb831 100644 --- a/ros2_controllers_test_nodes/setup.cfg +++ b/ros2_controllers_test_nodes/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/ros2_controllers_test_nodes +script_dir=$base/lib/ros2_controllers_test_nodes [install] -install-scripts=$base/lib/ros2_controllers_test_nodes +install_scripts=$base/lib/ros2_controllers_test_nodes From fa1138eafad644134b6887c9b456f962d4d5d838 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 Jul 2022 14:42:42 -0500 Subject: [PATCH 108/524] Preallocate JTC variables to avoid resizing in realtime loops (#340) --- .../joint_trajectory_controller.hpp | 16 +- .../src/joint_trajectory_controller.cpp | 154 ++++++++++-------- 2 files changed, 98 insertions(+), 72 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 dfb2358475..41bac54787 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 @@ -124,9 +124,17 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa hardware_interface::HW_IF_EFFORT, }; + // Preallocate variables used in the realtime update() function + trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint state_desired_; + trajectory_msgs::msg::JointTrajectoryPoint state_error_; + + // Degrees of freedom + size_t dof_; + // Parameters for some special cases, e.g. hydraulics powered robots - /// Run he controller in open-loop, i.e., read hardware states only when starting controller. - /// This is useful when robot is not exactly following the commanded trajectory. + // Run the controller in open-loop, i.e., read hardware states only when starting controller. + // This is useful when robot is not exactly following the commanded trajectory. bool open_loop_control_ = false; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Allow integration in goal trajectories to accept goals without position or velocity specified @@ -156,9 +164,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool use_closed_loop_pid_adapter_ = false; using PidPtr = std::shared_ptr; std::vector pids_; - /// Feed-forward velocity weight factor when calculating closed loop pid adapter's command + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; - /// reserved storage for result of the command when closed loop pid adapter is used + // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index aabc2fbb53..e407d8afa8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,7 +46,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}) +: controller_interface::ControllerInterface(), joint_names_({}), dof_(0) { } @@ -82,7 +82,16 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * command_interface_types_.size()); + if (dof_ == 0) + { + fprintf( + stderr, + "During ros2_control interface configuration, degrees of freedom is not valid;" + " it should be positive. Actual DOF is %zu\n", + dof_); + std::exit(EXIT_FAILURE); + } + conf.names.reserve(dof_ * command_interface_types_.size()); for (const auto & joint_name : joint_names_) { for (const auto & interface_type : command_interface_types_) @@ -98,7 +107,7 @@ JointTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + conf.names.reserve(dof_ * state_interface_types_.size()); for (const auto & joint_name : joint_names_) { for (const auto & interface_type : state_interface_types_) @@ -145,23 +154,19 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->update(*new_external_msg); } - JointTrajectoryPoint state_current, state_desired, state_error; - const auto joint_num = joint_names_.size(); - resize_joint_trajectory_point(state_current, joint_num); - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = - [&, joint_num](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < joint_num; ++index) + [&](auto & joint_interface, const std::vector & trajectory_point_interface) { + for (size_t index = 0; index < dof_; ++index) { joint_interface[index].get().set_value(trajectory_point_interface[index]); } }; // current state update - state_current.time_from_start.set__sec(0); - read_state_from_hardware(state_current); + state_current_.time_from_start.set__sec(0); + read_state_from_hardware(state_current_); // currently carrying out a trajectory if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) @@ -177,15 +182,14 @@ controller_interface::return_type JointTrajectoryController::update( } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); } } - resize_joint_trajectory_point(state_error, joint_num); // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = - (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr); + (*traj_point_active_ptr_)->sample(time, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { @@ -196,16 +200,16 @@ controller_interface::return_type JointTrajectoryController::update( const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); // Check state/goal tolerance - for (size_t index = 0; index < joint_num; ++index) + for (size_t index = 0; index < dof_; ++index) { - compute_error_for_joint(state_error, index, state_current, state_desired); + compute_error_for_joint(state_error_, index, state_current_, state_desired_); // Always check the state tolerance on the first sample in case the first sample // is the last point if ( (before_last_point || first_sample) && !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, default_tolerances_.state_tolerance[index], false)) { tolerance_violated_while_moving = true; } @@ -213,7 +217,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) { outside_goal_tolerance = true; @@ -239,12 +243,12 @@ controller_interface::return_type JointTrajectoryController::update( if (use_closed_loop_pid_adapter_) { // Update PIDs - for (auto i = 0ul; i < joint_num; ++i) + for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( - state_desired.positions[i] - state_current.positions[i], - state_desired.velocities[i] - state_current.velocities[i], + state_desired_.positions[i] - state_current_.positions[i], + state_desired_.velocities[i] - state_current_.velocities[i], (uint64_t)period.nanoseconds()); } } @@ -252,7 +256,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired.positions); + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); } if (has_velocity_command_interface_) { @@ -262,12 +266,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); } if (has_effort_command_interface_) { @@ -277,12 +281,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); } } - // store command as state when hardware state has tracking offset - last_commanded_state_ = state_desired; + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; } const auto active_goal = *rt_active_goal_.readFromRT(); @@ -293,9 +297,9 @@ controller_interface::return_type JointTrajectoryController::update( feedback->header.stamp = time; feedback->joint_names = joint_names_; - feedback->actual = state_current; - feedback->desired = state_desired; - feedback->error = state_error; + feedback->actual = state_current_; + feedback->desired = state_desired_; + feedback->error = state_error_; active_goal->setFeedback(feedback); // check abort @@ -346,16 +350,15 @@ controller_interface::return_type JointTrajectoryController::update( } } - publish_state(state_desired, state_current, state_error); + publish_state(state_desired_, state_current_, state_error_); return controller_interface::return_type::OK; } void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { - const auto joint_num = joint_names_.size(); auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < joint_num; ++index) + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { + for (size_t index = 0; index < dof_; ++index) { trajectory_point_interface[index] = joint_interface[index].get().get_value(); } @@ -391,10 +394,9 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto { bool has_values = true; - const auto joint_num = joint_names_.size(); auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < joint_num; ++index) + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { + for (size_t index = 0; index < dof_; ++index) { trajectory_point_interface[index] = joint_interface[index].get().get_value(); } @@ -462,6 +464,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // update parameters joint_names_ = get_node()->get_parameter("joints").as_string_array(); + if ((dof_ > 0) && (joint_names_.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; + } + + dof_ = joint_names_.size(); if (!reset()) { @@ -555,10 +568,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - size_t num_joints = joint_names_.size(); - pids_.resize(num_joints); - ff_velocity_scale_.resize(num_joints); - tmp_command_.resize(num_joints, 0.0); + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + tmp_command_.resize(dof_, 0.0); // Init PID gains from ROS parameter server for (size_t i = 0; i < pids_.size(); ++i) @@ -719,24 +731,22 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); - const auto n_joints = joint_names_.size(); - state_publisher_->lock(); state_publisher_->msg_.joint_names = joint_names_; - state_publisher_->msg_.desired.positions.resize(n_joints); - state_publisher_->msg_.desired.velocities.resize(n_joints); - state_publisher_->msg_.desired.accelerations.resize(n_joints); - state_publisher_->msg_.actual.positions.resize(n_joints); - state_publisher_->msg_.error.positions.resize(n_joints); + state_publisher_->msg_.desired.positions.resize(dof_); + state_publisher_->msg_.desired.velocities.resize(dof_); + state_publisher_->msg_.desired.accelerations.resize(dof_); + state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.error.positions.resize(dof_); if (has_velocity_state_interface_) { - state_publisher_->msg_.actual.velocities.resize(n_joints); - state_publisher_->msg_.error.velocities.resize(n_joints); + state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations.resize(n_joints); - state_publisher_->msg_.error.accelerations.resize(n_joints); + state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.error.accelerations.resize(dof_); } state_publisher_->unlock(); @@ -762,6 +772,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); + resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point(state_desired_, dof_); + resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(last_commanded_state_, dof_); + return CallbackReturn::SUCCESS; } @@ -778,8 +793,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -792,8 +807,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -822,14 +837,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset - resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); + 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 last_commanded_state_ from commands is + // Handle restart of controller by reading from commands if // those are not nan trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, joint_names_.size()); + resize_joint_trajectory_point(state, dof_); if (read_state_from_command_interfaces(state)) { + state_current_ = state; + state_desired_ = state; last_commanded_state_ = state; } @@ -841,7 +859,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { // TODO(anyone): How to halt when using effort commands? - for (size_t index = 0; index < joint_names_.size(); ++index) + for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) { @@ -1039,14 +1057,14 @@ void JointTrajectoryController::fill_partial_goal( { // joint names in the goal are a subset of existing joints, as checked in goal_callback // so if the size matches, the goal contains all controller joints - if (joint_names_.size() == trajectory_msg->joint_names.size()) + if (dof_ == trajectory_msg->joint_names.size()) { return; } - trajectory_msg->joint_names.reserve(joint_names_.size()); + trajectory_msg->joint_names.reserve(dof_); - for (size_t index = 0; index < joint_names_.size(); ++index) + for (size_t index = 0; index < dof_; ++index) { { if ( @@ -1162,7 +1180,7 @@ bool JointTrajectoryController::validate_trajectory_msg( // If partial joints goals are not allowed, goal should specify all controller joints if (!allow_partial_joints_goal_) { - if (trajectory.joint_names.size() != joint_names_.size()) + if (trajectory.joint_names.size() != dof_) { RCLCPP_ERROR( get_node()->get_logger(), @@ -1300,14 +1318,14 @@ bool JointTrajectoryController::contains_interface_type( void JointTrajectoryController::resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) { - point.positions.resize(size); + point.positions.resize(size, 0.0); if (has_velocity_state_interface_) { - point.velocities.resize(size); + point.velocities.resize(size, 0.0); } if (has_acceleration_state_interface_) { - point.accelerations.resize(size); + point.accelerations.resize(size, 0.0); } } From 7e1c23f6e94b3ee825815646c50d19dafab465dc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Jul 2022 08:48:56 +0100 Subject: [PATCH 109/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 12 files changed, 40 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 928627afdd..26525ff0af 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ea792565ca..3e9e52dee8 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 2b1ca87ab8..14d67dafb6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 91bd17982a..8945c6fba8 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8dcf1fd4b3..fd254c9728 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2c4ff3ffce..319e9dbcb8 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3730652f4e..9baa71d7c4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ * Update controllers with new get_name hardware interfaces (`#369 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ef8b23875b..affaf78b60 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Preallocate JTC variables to avoid resizing in realtime loops (`#340 `_) +* Contributors: Andy Zelenak + 2.7.0 (2022-07-03) ------------------ * Properly retrieve parameters in the Joint Trajectory Controller (`#365 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 05c1c8be15..d0e70b0d1b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6d2a5cad11..9308b2086b 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index bf159b7431..d5f39a65b8 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 +----------- +* Fix deprecation in setup.cfg on Jammy (Humble and Rolling). (`#375 `_) +* Contributors: Denis Štogl + 2.7.0 (2022-07-03) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c6334a85b2..3966b7a84f 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.7.0 (2022-07-03) ------------------ From bbfd8e1f71b6295d3dd9ca54b7eefd3203631bc9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Jul 2022 08:49:22 +0100 Subject: [PATCH 110/524] 2.8.0 --- 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 +- 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 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 25 files changed, 37 insertions(+), 37 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 26525ff0af..af6ede6ee6 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 6345e3a042..ffc961282d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.7.0 + 2.8.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 3e9e52dee8..0589e08993 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 01efb74b7f..1eb3eefed6 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.7.0 + 2.8.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 14d67dafb6..4ee77a3f03 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index e2b39122e2..1d0c3aab19 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.7.0 + 2.8.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 8945c6fba8..e9c4d48a2e 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 19c0b65d62..e4efa9fcf3 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.7.0 + 2.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fd254c9728..f55054a1ab 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 85416605ac..0c1562baf1 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.7.0 + 2.8.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 319e9dbcb8..764d6d4576 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index c7c4acaf10..0fd13191c8 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.7.0 + 2.8.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 9baa71d7c4..cd4233e612 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 3c2842fd10..9692f7d76c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.7.0 + 2.8.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 affaf78b60..8521d29f63 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ * Preallocate JTC variables to avoid resizing in realtime loops (`#340 `_) * Contributors: Andy Zelenak diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 385ad3d10b..bcf68555f5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.7.0 + 2.8.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 d0e70b0d1b..c0463698be 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index ce59d22ef2..26db59a027 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.7.0 + 2.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 9308b2086b..8aadf44a49 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 522db24b1c..3ed8250aaf 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.7.0 + 2.8.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 d5f39a65b8..0555c9783a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ * Fix deprecation in setup.cfg on Jammy (Humble and Rolling). (`#375 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0ed743ad9e..96fa6c2d40 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.7.0 + 2.8.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 eb23d114c8..f8a4ea5fb0 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.7.0", + version="2.8.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3966b7a84f..17bd1e6abc 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.8.0 (2022-07-09) +------------------ 2.7.0 (2022-07-03) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 7ad6052d35..64d62dea09 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.7.0 + 2.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From d1d86c7147be6ce8c505da44900a77ba5c1087cf Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 11 Jul 2022 02:39:24 -0500 Subject: [PATCH 111/524] Add option to skip interpolation in the joint trajectory controller (#374) * Introduce `InterpolationMethods` structure * Use parameters to define interpolation use in JTC --- .../interpolation_methods.hpp | 64 +++++ .../joint_trajectory_controller.hpp | 4 + .../trajectory.hpp | 6 +- .../src/joint_trajectory_controller.cpp | 8 +- .../src/trajectory.cpp | 47 ++-- .../test/test_trajectory.cpp | 237 +++++++++++++++--- 6 files changed, 316 insertions(+), 50 deletions(-) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp new file mode 100644 index 0000000000..1cebd56f8a --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ + +#include +#include +#include + +namespace joint_trajectory_controller +{ +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("joint_trajectory_controller.interpolation_methods"); + +namespace interpolation_methods +{ +enum class InterpolationMethod +{ + NONE, + VARIABLE_DEGREE_SPLINE +}; + +const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE; + +const std::unordered_map InterpolationMethodMap( + {{InterpolationMethod::NONE, "none"}, {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"}}); + +[[nodiscard]] inline InterpolationMethod from_string(const std::string & interpolation_method) +{ + if (interpolation_method.compare(InterpolationMethodMap.at(InterpolationMethod::NONE)) == 0) + { + return InterpolationMethod::NONE; + } + else if ( + !interpolation_method.compare( + InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) + { + return InterpolationMethod::VARIABLE_DEGREE_SPLINE; + } + // Default + else + { + RCLCPP_INFO_STREAM( + LOGGER, + "No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE."); + return InterpolationMethod::VARIABLE_DEGREE_SPLINE; + } +} +} // namespace interpolation_methods +} // namespace joint_trajectory_controller + +#endif // JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 41bac54787..13e5e3ff5f 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 @@ -27,6 +27,7 @@ #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" @@ -139,6 +140,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Allow integration in goal trajectories to accept goals without position or velocity specified bool allow_integration_in_goal_trajectories_ = false; + /// Specify interpolation method. Default to splines. + interpolation_methods::InterpolationMethod interpolation_method_{ + interpolation_methods::DEFAULT_INTERPOLATION}; double state_publish_rate_; double action_monitor_rate_; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7a4b495b70..7dac4ddbe1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -18,6 +18,7 @@ #include #include +#include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/time.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -75,13 +76,16 @@ class Trajectory * return false * * \param[in] sample_time Time at which trajectory will be sampled. + * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all. * \param[out] expected_state Calculated new at \p sample_time. * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above. * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state, + const rclcpp::Time & sample_time, + const interpolation_methods::InterpolationMethod interpolation_method, + trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e407d8afa8..235681b069 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -67,6 +67,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); state_publish_rate_ = auto_declare("state_publish_rate", 50.0); action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); + + std::string interpolation_string = auto_declare( + "interpolation_method", interpolation_methods::InterpolationMethodMap.at( + interpolation_methods::DEFAULT_INTERPOLATION)); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); } catch (const std::exception & e) { @@ -189,7 +194,8 @@ controller_interface::return_type JointTrajectoryController::update( // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = - (*traj_point_active_ptr_)->sample(time, state_desired_, start_segment_itr, end_segment_itr); + (*traj_point_active_ptr_) + ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 281302e5f4..a65329d289 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -58,7 +58,9 @@ void Trajectory::update(std::shared_ptr j } bool Trajectory::sample( - const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output_state, + const rclcpp::Time & sample_time, + const interpolation_methods::InterpolationMethod interpolation_method, + trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) @@ -88,22 +90,29 @@ bool Trajectory::sample( return false; } - // current time hasn't reached traj time of the first point in the msg yet auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; + // current time hasn't reached traj time of the first point in the msg yet if (sample_time < first_point_timestamp) { - // it changes points only if position and velocity are not exist, but their derivatives - deduce_from_derivatives( - state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), - (first_point_timestamp - time_before_traj_msg_).seconds()); - - interpolate_between_points( - time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, - sample_time, output_state); + // If interpolation is disabled, just forward the next waypoint + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) + { + output_state = state_before_traj_msg_; + } + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), + (first_point_timestamp - time_before_traj_msg_).seconds()); + interpolate_between_points( + time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, + sample_time, output_state); + } start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); return true; @@ -121,12 +130,20 @@ bool Trajectory::sample( if (sample_time >= t0 && sample_time < t1) { - // it changes points only if position and velocity are not exist, but their derivatives - deduce_from_derivatives( - point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); - - interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); + // If interpolation is disabled, just forward the next waypoint + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) + { + output_state = next_point; + } + // Do interpolation + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); + interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); + } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); return true; diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 3ad0388acd..cef8ad00dd 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -29,10 +29,14 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +using namespace joint_trajectory_controller::interpolation_methods; // NOLINT using namespace std::chrono_literals; +namespace +{ // Floating-point value comparison threshold const double EPS = 1e-8; +} // namespace TEST(TestTrajectory, initialize_trajectory) { @@ -45,7 +49,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), expected_point, start, end); + traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -61,7 +65,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), expected_point, start, end); + traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -106,7 +110,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { - traj.sample(time_now, expected_state, start, end); + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -116,14 +120,17 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample before time_now { - bool result = - traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); ASSERT_EQ(result, false); } // sample 0.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -134,7 +141,9 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample 1s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -144,7 +153,9 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample 1.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -153,20 +164,26 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample 2.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } // sample 3s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } // sample past given points { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -323,7 +340,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, expected_state, start, end); + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -333,14 +350,17 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample before time_now { - bool result = - traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(result, false); } // sample 0.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -358,7 +378,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) double position_first_seg = point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -370,7 +392,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample 1.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -388,7 +412,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) double position_second_seg = position_first_seg + (p1.velocities[0] + p2.velocities[0]) / 2 * (time_second_seg - time_first_seg); { - traj.sample(time_now + rclcpp::Duration::from_seconds(2), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -400,7 +426,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample 2.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -418,7 +446,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) double position_third_seg = position_second_seg + (p2.velocities[0] + p3.velocities[0]) / 2 * (time_third_seg - time_second_seg); { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -429,7 +459,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample past given points - movement virtually stops { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -481,7 +513,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { - traj.sample(time_now, expected_state, start, end); + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -492,14 +524,17 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample before time_now { - bool result = - traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(result, false); } // sample 0.5s after msg { - traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -517,7 +552,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho double position_first_seg = point_before_msg.positions[0] + (0.0 + p1.velocities[0]) / 2 * time_first_seg; { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -568,7 +605,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, expected_state, start, end); + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -579,8 +616,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample before time_now { - bool result = - traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end); + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(result, false); } @@ -592,7 +630,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) double position_first_seg = point_before_msg.positions[0] + (0.0 + velocity_first_seg) / 2 * time_first_seg; { - traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -606,7 +646,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) double position_second_seg = position_first_seg + (velocity_first_seg + velocity_second_seg) / 2 * (time_second_seg - time_first_seg); { - traj.sample(time_now + rclcpp::Duration::from_seconds(2), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -620,7 +662,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) double position_third_seg = position_second_seg + (velocity_second_seg + velocity_third_seg) / 2 * (time_third_seg - time_second_seg); { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, + end); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -630,7 +674,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample past given points - movement virtually stops { - traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end); + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, + start, end); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -638,3 +684,128 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) EXPECT_NEAR(p3.accelerations[0], expected_state.accelerations[0], EPS); } } + +TEST(TestTrajectory, skip_interpolation) +{ + // Simple passthrough without extra interpolation + { + const InterpolationMethod no_interpolation = InterpolationMethod::NONE; + + auto full_msg = std::make_shared(); + full_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + full_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + full_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + full_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + // set current state before trajectory msg was sent + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, full_msg); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // sample at trajectory starting time + { + traj.sample(time_now, no_interpolation, expected_state, start, end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample before time_now + { + bool result = traj.sample( + time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(result, false); + } + + // sample 0.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ(traj.begin(), end); + // For passthrough, this should just return the first waypoint + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample 1s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); + // There were no vels/accels in the input, so they should remain empty + EXPECT_EQ( + static_cast::type>(0), expected_state.velocities.size()); + EXPECT_EQ( + static_cast::type>(0), expected_state.accelerations.size()); + } + + // sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, + end); + ASSERT_EQ(traj.begin(), start); + ASSERT_EQ((++traj.begin()), end); + EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); + } + + // sample 2.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, + end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample 3s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, + end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + + // sample past given points + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, + end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } + } +} From d05a055ed4dae9ff2d27f44facc29fdd4ee3d36b Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Wed, 13 Jul 2022 11:03:29 -0600 Subject: [PATCH 112/524] Allow gripper stalling when moving to goal (#355) * Allow gripper stalling when moving to goal * update to debug prints * reduce comment line length * trim trailing whitespace --- .../gripper_action_controller.hpp | 1 + .../gripper_action_controller_impl.hpp | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 9f1b74d3ab..98ccbbf5d1 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -162,6 +162,7 @@ class GripperActionController : public controller_interface::ControllerInterface stall_velocity_threshold_; ///< Stall related parameters double default_max_effort_; ///< Max allowed effort double goal_tolerance_; + bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful /** * \brief Check for success and publish appropriate result and feedback. diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 748887f612..a92fc9d2a7 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -46,6 +46,7 @@ controller_interface::CallbackReturn GripperActionController: auto_declare("joint", joint_name_); auto_declare("goal_tolerance", 0.01); auto_declare("max_effort", 0.0); + auto_declare("allow_stalling", false); auto_declare("stall_velocity_threshold", 0.001); auto_declare("stall_timeout", 1.0); } @@ -164,6 +165,7 @@ void GripperActionController::check_for_success( pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); rt_active_goal_->setSucceeded(pre_alloc_result_); rt_active_goal_.reset(); } @@ -179,7 +181,16 @@ void GripperActionController::check_for_success( pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; - rt_active_goal_->setAborted(pre_alloc_result_); + if(allow_stalling_) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + rt_active_goal_->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + rt_active_goal_->setAborted(pre_alloc_result_); + } rt_active_goal_.reset(); } } @@ -212,6 +223,9 @@ controller_interface::CallbackReturn GripperActionController: // Max allowable effort default_max_effort_ = get_node()->get_parameter("max_effort").as_double(); default_max_effort_ = fabs(default_max_effort_); + // Allow stalling will make the action server return success if the + // gripper stalls when moving to the goal + allow_stalling_ = get_node()->get_parameter("allow_stalling").as_bool(); // Stall - stall velocity threshold, stall timeout stall_velocity_threshold_ = get_node()->get_parameter("stall_velocity_threshold").as_double(); stall_timeout_ = get_node()->get_parameter("stall_timeout").as_double(); From 71231103cc4ee50aae80a6f06a54c9c9d45ba690 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 14 Jul 2022 08:34:32 +0100 Subject: [PATCH 113/524] Update changelogs --- 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 | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 12 files changed, 42 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index af6ede6ee6..5e74bda2e7 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0589e08993..10b14c12f4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4ee77a3f03..ddff017a5d 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e9c4d48a2e..1e3ed31671 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f55054a1ab..8e9b071ae8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Allow gripper stalling when moving to goal (`#355 `_) +* Contributors: Marq Rasmussen + 2.8.0 (2022-07-09) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 764d6d4576..35f0428bbb 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cd4233e612..299b551d74 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8521d29f63..6b63165cf1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add option to skip interpolation in the joint trajectory controller (`#374 `_) + * Introduce `InterpolationMethods` structure + * Use parameters to define interpolation use in JTC +* Contributors: Andy Zelenak + 2.8.0 (2022-07-09) ------------------ * Preallocate JTC variables to avoid resizing in realtime loops (`#340 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c0463698be..8d1be7b590 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8aadf44a49..4c0ccd2024 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0555c9783a..1515e774d3 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ * Fix deprecation in setup.cfg on Jammy (Humble and Rolling). (`#375 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 17bd1e6abc..e0de4c03d4 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.8.0 (2022-07-09) ------------------ From 59cf3448af2c2974cf205001cd3d8afad8a8194f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 14 Jul 2022 08:34:59 +0100 Subject: [PATCH 114/524] 2.9.0 --- 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 +- 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 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 25 files changed, 37 insertions(+), 37 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5e74bda2e7..54d3e19561 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index ffc961282d..df90159f64 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.8.0 + 2.9.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 10b14c12f4..d642cba146 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 1eb3eefed6..d766cdbef3 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.8.0 + 2.9.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 ddff017a5d..4c6cb3cf70 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1d0c3aab19..d5b3e9a3ea 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.8.0 + 2.9.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 1e3ed31671..ee4131d576 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index e4efa9fcf3..d9d60c95cd 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.8.0 + 2.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8e9b071ae8..5c9c2bcc8e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ * Allow gripper stalling when moving to goal (`#355 `_) * Contributors: Marq Rasmussen diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 0c1562baf1..27586a3e97 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.8.0 + 2.9.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 35f0428bbb..13014b5a62 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 0fd13191c8..6a4c01c3c5 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.8.0 + 2.9.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 299b551d74..862c663a5d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9692f7d76c..9b546b81a3 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.8.0 + 2.9.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 6b63165cf1..ca856cdc28 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ * Add option to skip interpolation in the joint trajectory controller (`#374 `_) * Introduce `InterpolationMethods` structure * Use parameters to define interpolation use in JTC diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index bcf68555f5..047e778160 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.8.0 + 2.9.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 8d1be7b590..ec225f9c9f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 26db59a027..aa29cc5ea5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.8.0 + 2.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4c0ccd2024..0aa1a78d34 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3ed8250aaf..0f6f2b31d8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.8.0 + 2.9.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 1515e774d3..b30e88623f 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 96fa6c2d40..c6c5a6b457 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.8.0 + 2.9.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 f8a4ea5fb0..e10da150bb 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.8.0", + version="2.9.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e0de4c03d4..24e4c10e5a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.9.0 (2022-07-14) +------------------ 2.8.0 (2022-07-09) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 64d62dea09..58d2f7f81b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.8.0 + 2.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 001d896e73297a4913999da47449dd835e8c8b9e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 23 Jul 2022 10:07:17 -0700 Subject: [PATCH 115/524] Parameter loading fixup in diff_drive and gripper controllers (#385) * Parameter loading fixup * Lint --- diff_drive_controller/src/diff_drive_controller.cpp | 7 ++++--- .../gripper_controllers/gripper_action_controller_impl.hpp | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index bc2e7eed22..aec603ad22 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -77,9 +77,10 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - auto_declare("publish_limited_velocity", publish_limited_velocity_); + publish_limited_velocity_ = auto_declare("publish_limited_velocity", + publish_limited_velocity_); auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); + use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); auto_declare("linear.x.has_velocity_limits", false); auto_declare("linear.x.has_acceleration_limits", false); @@ -100,7 +101,7 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("angular.z.min_acceleration", NAN); auto_declare("angular.z.max_jerk", NAN); auto_declare("angular.z.min_jerk", NAN); - auto_declare("publish_rate", publish_rate_); + publish_rate_ = auto_declare("publish_rate", publish_rate_); } catch (const std::exception & e) { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index a92fc9d2a7..f1d270b52f 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -43,7 +43,7 @@ controller_interface::CallbackReturn GripperActionController: { // with the lifecycle node being initialized, we can declare parameters auto_declare("action_monitor_rate", 20.0); - auto_declare("joint", joint_name_); + joint_name_ = auto_declare("joint", joint_name_); auto_declare("goal_tolerance", 0.01); auto_declare("max_effort", 0.0); auto_declare("allow_stalling", false); From d8d661607f770e704b774249a31534fa0eb0d46b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 24 Jul 2022 08:43:40 +0200 Subject: [PATCH 116/524] Use system time in all tests to avoid error with different time sources. (#334) --- .../test/test_trajectory_controller.cpp | 22 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 2 +- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 57b0a36251..e4be504717 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -78,7 +78,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); std::this_thread::sleep_for(std::chrono::milliseconds(10)); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -141,7 +141,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // time_from_start.sec = 1; // time_from_start.nanosec = 0; // std::vector> points {{{3.3, 4.4, 5.5}}}; -// publish(time_from_start, points); +// publish(time_from_start, points, rclcpp::Time()); // // wait for msg is be published to the system // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // executor.spin_once(); @@ -189,7 +189,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // {{10.10, 11.11, 12.12}} // }; // // *INDENT-ON* -// publish(time_from_start, points); +// publish(time_from_start, points, rclcpp::Time()); // // wait for msg is be published to the system // std::this_thread::sleep_for(std::chrono::milliseconds(500)); // executor.spin_once(); @@ -241,7 +241,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -847,9 +847,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}}}; + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time()); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; @@ -861,6 +862,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; + std::cout << "Sending old trajectory" << std::endl; publish(time_from_start, points_new, new_traj_start); waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -876,7 +878,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time()); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; @@ -1014,7 +1016,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // Move joint back to the first goal points = {{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there should be a "jump" in the goal direction from command @@ -1057,7 +1059,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); @@ -1074,7 +1076,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there **should not** be a "jump" in opposite direction from command @@ -1101,7 +1103,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint back to the first goal points = {{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there **should not** be a "jump" in the goal direction from command diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c4becd25d6..5c55d87972 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -279,7 +279,7 @@ class TrajectoryControllerTest : public ::testing::Test */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, rclcpp::Time start_time = rclcpp::Time(), + const std::vector> & points, rclcpp::Time start_time, const std::vector & joint_names = {}, const std::vector> & points_velocities = {}) { From 4504e8d36157770b024997208975cbc40045f050 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= <48183611+Noel215@users.noreply.github.com> Date: Fri, 29 Jul 2022 09:19:59 +0200 Subject: [PATCH 117/524] port rqt_joint_trajectory_controller to ros2 (#356) --- rqt_joint_trajectory_controller/package.xml | 35 ++ rqt_joint_trajectory_controller/plugin.xml | 21 + .../resource/double_editor.ui | 95 ++++ .../resource/joint_trajectory_controller.ui | 202 +++++++ .../resource/off.svg | 267 ++++++++++ .../resource/on.svg | 231 ++++++++ .../resource/rqt_joint_trajectory_controller | 0 .../resource/scope.png | Bin 0 -> 3205 bytes .../resource/scope.svg | 269 ++++++++++ .../__init__.py | 0 .../double_editor.py | 110 ++++ .../joint_limits_urdf.py | 100 ++++ .../joint_trajectory_controller.py | 500 ++++++++++++++++++ .../rqt_joint_trajectory_controller.py | 8 + .../update_combo.py | 57 ++ .../rqt_joint_trajectory_controller/utils.py | 416 +++++++++++++++ rqt_joint_trajectory_controller/setup.cfg | 4 + rqt_joint_trajectory_controller/setup.py | 33 ++ 18 files changed, 2348 insertions(+) create mode 100644 rqt_joint_trajectory_controller/package.xml create mode 100644 rqt_joint_trajectory_controller/plugin.xml create mode 100644 rqt_joint_trajectory_controller/resource/double_editor.ui create mode 100644 rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui create mode 100644 rqt_joint_trajectory_controller/resource/off.svg create mode 100644 rqt_joint_trajectory_controller/resource/on.svg create mode 100644 rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller create mode 100644 rqt_joint_trajectory_controller/resource/scope.png create mode 100644 rqt_joint_trajectory_controller/resource/scope.svg create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py create mode 100755 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py create mode 100644 rqt_joint_trajectory_controller/setup.cfg create mode 100644 rqt_joint_trajectory_controller/setup.py diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml new file mode 100644 index 0000000000..343f733339 --- /dev/null +++ b/rqt_joint_trajectory_controller/package.xml @@ -0,0 +1,35 @@ + + + + rqt_joint_trajectory_controller + 2.5.0 + Graphical frontend for interacting with joint_trajectory_controller instances. + + Bence Magyar + Mathias Lüdtke + Enrique Fernandez + + Apache-2.0 + + http://wiki.ros.org/rqt_joint_trajectory_controller + + Adolfo Rodriguez Tsouroukdissian + Noel Jimenez Garcia + + control_msgs + controller_manager_msgs + python_qt_binding + python3-rospkg + trajectory_msgs + rclpy + rqt_gui + rqt_gui_py + qt_gui + + + ament_python + + + diff --git a/rqt_joint_trajectory_controller/plugin.xml b/rqt_joint_trajectory_controller/plugin.xml new file mode 100644 index 0000000000..4feade126e --- /dev/null +++ b/rqt_joint_trajectory_controller/plugin.xml @@ -0,0 +1,21 @@ + + + + Graphical frontend for interacting with joint_trajectory_controller instances. + + + + + folder + Plugins related to robot tools. + + + resource/scope.png + + Monitor and send commands to running joint trajectory controllers. + + + + diff --git a/rqt_joint_trajectory_controller/resource/double_editor.ui b/rqt_joint_trajectory_controller/resource/double_editor.ui new file mode 100644 index 0000000000..547a90220f --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/double_editor.ui @@ -0,0 +1,95 @@ + + + double_editor + + + true + + + + 0 + 0 + 249 + 121 + + + + + 0 + 0 + + + + Form + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + 100 + + + Qt::Horizontal + + + false + + + false + + + + + + + true + + + + 0 + 0 + + + + + 60 + 0 + + + + + 100 + 0 + + + + QAbstractSpinBox::UpDownArrows + + + false + + + 6 + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui new file mode 100644 index 0000000000..3f3b6c1186 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -0,0 +1,202 @@ + + + joint_trajectory_controller + + + + 0 + 0 + 336 + 317 + + + + Joint trajectory controller + + + + + + + + + + + + + + + + + + + + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + + + controller manager ns + + + cm_combo + + + + + + + controller + + + jtc_combo + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + <html><head/><body><p>Enable/disable sending commands to the controller.</p></body></html> + + + + + + + off.svg + on.svg + on.svgoff.svg + + + + 48 + 48 + + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + joints + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + false + + + false + + + + + + + 0 + 0 + + + + QFrame::NoFrame + + + QFrame::Plain + + + Qt::ScrollBarAsNeeded + + + Qt::ScrollBarAlwaysOff + + + true + + + + + 0 + 0 + 314 + 109 + + + + + + + + + + + + speed scaling + + + + + + + + cm_combo + jtc_combo + enable_button + + + + diff --git a/rqt_joint_trajectory_controller/resource/off.svg b/rqt_joint_trajectory_controller/resource/off.svg new file mode 100644 index 0000000000..4c91a11260 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/off.svg @@ -0,0 +1,267 @@ + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + red power button + 08 12 2006 + + + molumen + + + red power button + + + + icon + button + design + UI + interface + power + switch + on + off + red + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/on.svg b/rqt_joint_trajectory_controller/resource/on.svg new file mode 100644 index 0000000000..233d356054 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/on.svg @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + image/svg+xml + + green power button + 08 12 2006 + + + molumen + + + green power button + + + + icon + button + design + UI + interface + power + switch + on + off + green + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller b/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/resource/scope.png b/rqt_joint_trajectory_controller/resource/scope.png new file mode 100644 index 0000000000000000000000000000000000000000..24c089b70a3d2f984edd161dc746b54a250c6af4 GIT binary patch literal 3205 zcmV;040`j4P)T-R~Oe|64%^_$%@!y!eAvK5PBAik=o^$?H^{-QPrp0^D*I9zcDc^YG4gJCkFN9y!X12*&Z@rcK z$5>g;>)w3x&7C-oZ+q`w6_G#m-v3?!h{zQ9Z|B@UD5d`9_U+qdO}`4@&Ye52dGG%Y zxbhY6^GW4rBJvlvZ{NPR7+3yv{cMD z!yUd`yN)0L?<%EUy?y((gB1>xW%<`4(o22G-;RFD$Yoz{<=$OqvC)cw*DC4 zW#`;q0zU#!%k{kfQ|BB%Nq^1QXNd6S9`)OOKCvf!SR62qm_J+WJAjI~-uoK>pV?E8 z%6y&ukf^rAxF%t&D08tGawz0myxca}E(n)&KxFS553!`6%;eo1D`rQG`06nKWp$BkECt z2d4RqVKyNQ9D+If=KMO_9lfUWQh9SVa|QremZ5cJu|*8SJpL{Ku=bt^q0+RH8vBhN zd#yI#{a(go67kVTm-yx9Clv9Rkt$HiasKRCo$GPyfL|JV4FIs5RAj|Sb26S6&)h0= zol-&^hIAWkZa%fcw_i9#2ae88#{N?Q<-hLn(0h)8l)~5z04Scw(;hYeEVt*J!#Rh? zuZM?hJ`0c{)#d}GX~YS8%}adinF6If4?lR3ga3PxAT+%6ZAUL@lLQf332Odmr3kbl z5QWwXrDZb%t!6aw+e2sp06@ZXA++j&6~irDt3-%`fM#50|5B4?_lV)cT|PUUqD0ZS z+M>5VqjUEX^(bbV&%pVLGb*Tf6NjXcP}mad9Bw65$*KpuUyyhcz!H`CWg3m@Lhsu4 zQmC~Xfbs-7pb^%&)@t+oEsqk<@u0yZ?xRF8nKfwb47vVv%)_*Wn&y<&5C;+Ms7WiT z)9i-y8Y71Jgqh7S)>M(OG@_py?*b02k(e_Nl&PctaC>?K7(6NoltXF>)&(?bDP>;c z*B?*0KRiN-}}3ak1Vrs%B(E0z9M_? zYINoW%Laco)94C0SBbhRB4T!)m zoFE8S+k3qJYEHeGf>-?D`@-GGA(LB7LaD39hImNYWhIXw<97brR5hd&Eaz^AQ3!#-$ za5YUU4T7MxA_@a)QB0>>;;g1WapbPRc!L(j%w!y$z_)-adxFpRsn;E?okyHJ+~vXO zm`)JW=(o7EJE5JlX+#Ora)$M?#>ponaTP`YQKINwjW{nlfTSK!s|nqt#^p;*+TAI6 zR!282wH;5psS$)IvCOo^lnK3S$0Xb!+4~R>K0i*F#C?J&VK{Ek-WhW9xtLkp!B29g zMT!Letdw}%NxaCfN-MOgzHlXs4$Ecw_uk|21d*cN1Tz+D4Udn6C{8)J-{bUj#_^*W zp%En%iNbl0bq3=s&U>^LlvEigiDLG4DmM>?0ht|QFnDoz zpl~_;e8>k68l0YVxw>ySIu7{o?v#VcG2>!Jtq^n>(?4$0yF6sCxlccBQCdTyJ+;tP z>ngn+KoA6CttAXYy0s2c4+vF2rxnx8b4q7O%Zy2pl9m~w90-I#Mc{6ya_ey)a#vuz zT~>~zGmP>HkEX|bc3%^H5>h(DB%d-ZQjD{tE`nmhz56Y8uXvul0n=iiFbD{m4n3Pu zIzzoo&{|`yUC`180AU!4bB-iQci_nR}_g6cau>O1U+zaplGlWqyOb zVGZ4I^n)3*G9wgEbQF>2xvY7>S}P)g)|&5przDIvo0yx=EB@=}d%SZUVWYoy1&u<}NSX%6K&B;_{rC&8VrE zRy`z)OO6lQ zA3$i#W;I%trMK2Holg1q@EAW<1WL2#8mKa0uihg$z0A)318!Wsf*H4%ltNP)T)nCh zpl}g!*HI6It_`t%y>)E$0=(Z$$=*Y=t5FI9Wyti1LorMa6CxE6go^$_Ms5pQhl*#j z7G+ub#mq|pixX~`jVN#+LNT6F^o4PwpvF_|>_ew+kfVq))fiFKg6iJr^M{7R!wFeE zoj&hVJl%xb|j8*|!6qoj529s(&pVrB;aN&V7007onLnG}VoyXz4P`WC6 z2@sW*Sy=%uOn|j^drzzuhnMmd2>~Y_;|;khn3;_6s79wdp_%NHnUaR8ku(NmSwc21 zADsBj3f8k$&umAqc-1d^0RW4`GT#qwkwTZUS&gzZtP{!o~i zjB*aZ%SvDU1^xAJq+2ocLN=;V<{=h zdqX0Xp^{@J!zPc81{@7X4D%@s7ce^6ycpI2YvT(H6?r|Q)9o3mvm1k|-u7}qN0w#T zS!^x(fw|kfRL4)|3WmjqU)@*W1$FtSpe%PE1q_N2nagmv?cuW}OwRZr^Adi0Bem94 z@g&zOoNFiN-IwttqjJorgCp9X?(y_q%7cTLUpzWtP>d+O!7tj;ZGH6lE1HD`=f+y< z&V|?c^!tT3bK5Ld)iaK%NjaDu^TCnj{kw|XoH8mVOw1HJ2XJA!yl7m_?_*cjJf~mD zm5*;|st^3q8tc}q3bKira#Fx-T4Lu1j&;>N&bqs|?0?I+QUZQk?Xwj8xw~eC zEeA~D9W$3QD)aUCSHIbI(|pktjeg8G=k{~Hp+&L*jwD_+)(2N3B0!wv6c94Qbrhi*)}_OKgkK zdF7Q?x+3xc@RZjuxK)x}wz%;3`%?rxQ#3x;v_7t)T}1x*op;{(^M!uxAKfp%{POoi z#x7wwATKWbM93Uc?oEI)w?_=CnEBn-ur)i r_uY4YbpCe}8vwp;^0@5#wUGY@C17*|Z}9e100000NkvXXu0mjf>V71? literal 0 HcmV?d00001 diff --git a/rqt_joint_trajectory_controller/resource/scope.svg b/rqt_joint_trajectory_controller/resource/scope.svg new file mode 100644 index 0000000000..ee4d4da371 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/scope.svg @@ -0,0 +1,269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py new file mode 100644 index 0000000000..23a1f2c806 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from ament_index_python.packages import get_package_share_directory + +from python_qt_binding import loadUi +from python_qt_binding.QtCore import Signal +from python_qt_binding.QtWidgets import QWidget + + +class DoubleEditor(QWidget): + # TODO: + # - Actually make bounds optional + # + # - Support wrapping mode + # + # - Support unspecified (+-Inf) lower and upper bounds (both, or one) + # + # - Allow to specify the step and page increment sizes + # (right-click context menu?) + # + # - Use alternative widget to slider for values that wrap, or are + # unbounded. + # QwtWheel could be a good choice, dials are not so good because they + # use lots of vertical (premium) screen space, and are fine for wrapping + # values, but not so much for unbounded ones + # + # - Merge with existing similar code such as rqt_reconfigure's + # DoubleEditor? + """ + Widget that allows to edit the value of a floating-point value. + Optionally subject to lower and upper bounds. + """ + + valueChanged = Signal(float) + + def __init__(self, min_val, max_val): + super(DoubleEditor, self).__init__() + + # Preconditions + assert min_val < max_val + + # Cache values + self._min_val = min_val + self._max_val = max_val + + # Load editor UI + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', 'double_editor.ui') + loadUi(ui_file, self) + + # Setup widget ranges and slider scale factor + self.slider.setRange(0, 100) + self.slider.setSingleStep(1) + self._scale = (max_val - min_val) / \ + (self.slider.maximum() - self.slider.minimum()) + self.spin_box.setRange(min_val, max_val) + self.spin_box.setSingleStep(self._scale) + + # Couple slider and spin box together + self.slider.valueChanged.connect(self._on_slider_changed) + self.spin_box.valueChanged.connect(self._on_spinbox_changed) + + # Ensure initial sync of slider and spin box + self._on_spinbox_changed() + + def _slider_to_val(self, sval): + return self._min_val + self._scale * (sval - self.slider.minimum()) + + def _val_to_slider(self, val): + return round(self.slider.minimum() + (val - self._min_val) / + self._scale) + + def _on_slider_changed(self): + val = self._slider_to_val(self.slider.value()) + self.spin_box.blockSignals(True) # Prevents updating the command twice + self.spin_box.setValue(val) + self.spin_box.blockSignals(False) + self.valueChanged.emit(val) + + def _on_spinbox_changed(self): + val = self.spin_box.value() + self.slider.blockSignals(True) # Prevents updating the command twice + self.slider.setValue(self._val_to_slider(val)) + self.slider.blockSignals(False) + self.valueChanged.emit(val) + + def setValue(self, val): + if val != self.spin_box.value(): + self.spin_box.blockSignals(True) + self.spin_box.setValue(val) # Update spin box first + self._on_spinbox_changed() # Sync slider with spin box + self.spin_box.blockSignals(False) + + def value(self): + return self.spin_box.value() diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py new file mode 100644 index 0000000000..6f339ba387 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python + +# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got +# Exception: Required attribute not set in XML: upper +# upper is an optional attribute, so I don't understand what's going on +# See comments in https://github.com/ros/urdfdom/issues/36 + +import xml.dom.minidom +from math import pi + +import rclpy +from std_msgs.msg import String + +description = "" + + +def callback(msg): + global description + description = msg.data + + +def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + # Code inspired on the joint_state_publisher package by David Lu!!! + # https://github.com/ros/robot_model/blob/indigo-devel/ + # joint_state_publisher/joint_state_publisher/joint_state_publisher + + qos_profile = rclpy.qos.QoSProfile(depth=1) + qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL + qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE + + node.create_subscription(String, "/robot_description", callback, qos_profile) + rclpy.spin_once(node) + + free_joints = {} + dependent_joints = {} + + if description != "": + robot = xml.dom.minidom.parseString(description)\ + .getElementsByTagName('robot')[0] + + # Find all non-fixed joints + for child in robot.childNodes: + if child.nodeType is child.TEXT_NODE: + continue + if child.localName == 'joint': + jtype = child.getAttribute('type') + if jtype == 'fixed': + continue + name = child.getAttribute('name') + try: + limit = child.getElementsByTagName('limit')[0] + except: + continue + if jtype == 'continuous': + minval = -pi + maxval = pi + else: + try: + minval = float(limit.getAttribute('lower')) + maxval = float(limit.getAttribute('upper')) + except: + continue + try: + maxvel = float(limit.getAttribute('velocity')) + except: + continue + safety_tags = child.getElementsByTagName('safety_controller') + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute('soft_lower_limit'): + minval = max(minval, + float(tag.getAttribute('soft_lower_limit'))) + if tag.hasAttribute('soft_upper_limit'): + maxval = min(maxval, + float(tag.getAttribute('soft_upper_limit'))) + + mimic_tags = child.getElementsByTagName('mimic') + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {'parent': tag.getAttribute('joint')} + if tag.hasAttribute('multiplier'): + entry['factor'] = float(tag.getAttribute('multiplier')) + if tag.hasAttribute('offset'): + entry['offset'] = float(tag.getAttribute('offset')) + + dependent_joints[name] = entry + continue + + if name in dependent_joints: + continue + + joint = {'min_position': minval, 'max_position': maxval} + joint["has_position_limits"] = jtype != 'continuous' + joint['max_velocity'] = maxvel + free_joints[name] = joint + return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py new file mode 100644 index 0000000000..f3c98f9fe0 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -0,0 +1,500 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import print_function +import os +import rclpy +import threading +from ament_index_python.packages import get_package_share_directory + +from qt_gui.plugin import Plugin +from python_qt_binding import loadUi +from python_qt_binding.QtCore import QTimer, Signal +from python_qt_binding.QtWidgets import QWidget, QFormLayout + +from control_msgs.msg import JointTrajectoryControllerState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from .utils import ControllerLister, ControllerManagerLister +from .double_editor import DoubleEditor +from .joint_limits_urdf import get_joint_limits +from .update_combo import update_combo + +# TODO: +# - Better UI suppor for continuous joints (see DoubleEditor TODO) +# - Can we load controller joints faster?, it's currently pretty slow +# - If URDF is reloaded, allow to reset the whole plugin? +# - Allow to configure: +# - URDF location +# - Command publishing and state update frequency +# - Controller manager and jtc monitor frequency +# - Min trajectory duration +# - Fail gracefully when the URDF or some other requisite is not set +# - Could users confuse the enable/disable button with controller start/stop +# (in a controller manager sense)? +# - Better decoupling between model and view +# - Tab order is wrong. For the record, this did not work: +# QWidget.setTabOrder(self._widget.controller_group, +# self._widget.joint_group) +# QWidget.setTabOrder(self._widget.joint_group, +# self._widget.speed_scaling_group) + +# NOTE: +# Controller enable/disable icons are in the public domain, and available here: +# freestockphotos.biz/photos.php?c=all&o=popular&s=0&lic=all&a=all&set=powocab_u2 + + +class JointTrajectoryController(Plugin): + """ + Graphical frontend for a C{JointTrajectoryController}. + + There are two modes for interacting with a controller: + 1. B{Monitor mode} Joint displays are updated with the state reported + by the controller. This is a read-only mode and does I{not} send + control commands. Every time a new controller is selected, it starts + in monitor mode for safety reasons. + + 2. B{Control mode} Joint displays update the control command that is + sent to the controller. Commands are sent periodically evan if the + displays are not being updated by the user. + + To control the aggressiveness of the motions, the maximum speed of the + sent commands can be scaled down using the C{Speed scaling control} + + This plugin is able to detect and keep track of all active controller + managers, as well as the JointTrajectoryControllers that are I{running} + in each controller manager. + For a controller to be compatible with this plugin, it must comply with + the following requisites: + - The controller type contains the C{JointTrajectoryController} + substring, e.g., C{position_controllers/JointTrajectoryController} + - The controller exposes the C{command} and C{state} topics in its + ROS interface. + + Additionally, there must be a URDF loaded with a valid joint limit + specification, namely position (when applicable) and velocity limits. + + A reference implementation of the C{JointTrajectoryController} is + available in the C{joint_trajectory_controller} ROS package. + """ + + _cmd_pub_freq = 10.0 # Hz + _widget_update_freq = 30.0 # Hz + _ctrlrs_update_freq = 1 # Hz + _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration + + jointStateChanged = Signal([dict]) + + def __init__(self, context): + super(JointTrajectoryController, self).__init__(context) + self.setObjectName('JointTrajectoryController') + self._node = rclpy.node.Node("rqt_joint_trajectory_controller") + self._executor = None + self._executor_thread = None + + # Create QWidget and extend it with all the attributes and children + # from the UI file + self._widget = QWidget() + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', + 'joint_trajectory_controller.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('JointTrajectoryControllerUi') + ns = self._node.get_namespace()[1:-1] + self._widget.controller_group.setTitle('ns: ' + ns) + + # Setup speed scaler + speed_scaling = DoubleEditor(1.0, 100.0) + speed_scaling.spin_box.setSuffix('%') + speed_scaling.spin_box.setValue(50.0) + speed_scaling.spin_box.setDecimals(0) + speed_scaling.setEnabled(False) + self._widget.speed_scaling_layout.addWidget(speed_scaling) + self._speed_scaling_widget = speed_scaling + speed_scaling.valueChanged.connect(self._on_speed_scaling_change) + self._on_speed_scaling_change(speed_scaling.value()) + + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + + # Initialize members + self._jtc_name = [] # Name of selected joint trajectory controller + self._cm_ns = [] # Namespace of the selected controller manager + self._joint_pos = {} # name->pos map for joints of selected controller + self._joint_names = [] # Ordered list of selected controller joints + self._robot_joint_limits = {} # Lazily evaluated on first use + + # Timer for sending commands to active controller + self._update_cmd_timer = QTimer(self) + self._update_cmd_timer.setInterval(int(1000.0 / self._cmd_pub_freq)) + self._update_cmd_timer.timeout.connect(self._update_cmd_cb) + + # Timer for updating the joint widgets from the controller state + self._update_act_pos_timer = QTimer(self) + self._update_act_pos_timer.setInterval(int(1000.0 / + self._widget_update_freq)) + self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) + + # Timer for controller manager updates + self._list_cm = ControllerManagerLister() + self._update_cm_list_timer = QTimer(self) + self._update_cm_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_cm_list_timer.timeout.connect(self._update_cm_list) + self._update_cm_list_timer.start() + + # Timer for running controller updates + self._update_jtc_list_timer = QTimer(self) + self._update_jtc_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) + self._update_jtc_list_timer.start() + + # Signal connections + w = self._widget + w.enable_button.toggled.connect(self._on_jtc_enabled) + w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) + w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) + + self._cmd_pub = None # Controller command publisher + self._state_sub = None # Controller state subscriber + + self._list_controllers = None + + def shutdown_plugin(self): + self._update_cmd_timer.stop() + self._update_act_pos_timer.stop() + self._update_cm_list_timer.stop() + self._update_jtc_list_timer.stop() + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('cm_ns', self._cm_ns) + instance_settings.set_value('jtc_name', self._jtc_name) + + def restore_settings(self, plugin_settings, instance_settings): + # Restore last session's controller_manager, if present + self._update_cm_list() + cm_ns = instance_settings.value('cm_ns') + cm_combo = self._widget.cm_combo + cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] + try: + idx = cm_list.index(cm_ns) + cm_combo.setCurrentIndex(idx) + # Resore last session's controller, if running + self._update_jtc_list() + jtc_name = instance_settings.value('jtc_name') + jtc_combo = self._widget.jtc_combo + jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] + try: + idx = jtc_list.index(jtc_name) + jtc_combo.setCurrentIndex(idx) + except (ValueError): + pass + except (ValueError): + pass + + # def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog + + def _update_cm_list(self): + update_combo(self._widget.cm_combo, self._list_cm()) + + def _update_jtc_list(self): + # Clear controller list if no controller information is available + if not self._list_controllers: + self._widget.jtc_combo.clear() + return + + # List of running controllers with a valid joint limits specification + # for _all_ their joints + running_jtc = self._running_jtc_info() + if running_jtc and not self._robot_joint_limits: + self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + valid_jtc = [] + if (self._robot_joint_limits): + for jtc_info in running_jtc: + has_limits = all(name in self._robot_joint_limits + for name in _jtc_joint_names(jtc_info)) + if has_limits: + valid_jtc.append(jtc_info) + valid_jtc_names = [data.name for data in valid_jtc] + + # Update widget + update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) + + def _on_speed_scaling_change(self, val): + self._speed_scale = val / self._speed_scaling_widget.slider.maximum() + + def _on_joint_state_change(self, actual_pos): + assert(len(actual_pos) == len(self._joint_pos)) + for name in actual_pos.keys(): + self._joint_pos[name]['position'] = actual_pos[name] + + def _on_cm_change(self, cm_ns): + self._cm_ns = cm_ns + if cm_ns: + self._list_controllers = ControllerLister(cm_ns) + # NOTE: Clear below is important, as different controller managers + # might have controllers with the same name but different + # configurations. Clearing forces controller re-discovery + self._widget.jtc_combo.clear() + self._update_jtc_list() + else: + self._list_controllers = None + + def _on_jtc_change(self, jtc_name): + self._unload_jtc() + self._jtc_name = jtc_name + if self._jtc_name: + self._load_jtc() + + def _on_jtc_enabled(self, val): + # Don't allow enabling if there are no controllers selected + if not self._jtc_name: + self._widget.enable_button.setChecked(False) + return + + # Enable/disable joint displays + for joint_widget in self._joint_widgets(): + joint_widget.setEnabled(val) + + # Enable/disable speed scaling + self._speed_scaling_widget.setEnabled(val) + + if val: + # Widgets send desired position commands to controller + self._update_act_pos_timer.stop() + self._update_cmd_timer.start() + else: + # Controller updates widgets with actual position + self._update_cmd_timer.stop() + self._update_act_pos_timer.start() + + def _load_jtc(self): + # Initialize joint data corresponding to selected controller + running_jtc = self._running_jtc_info() + self._joint_names = next(_jtc_joint_names(x) for x in running_jtc + if x.name == self._jtc_name) + for name in self._joint_names: + self._joint_pos[name] = {} + + # Update joint display + try: + layout = self._widget.joint_group.layout() + for name in self._joint_names: + limits = self._robot_joint_limits[name] + joint_widget = DoubleEditor(limits['min_position'], + limits['max_position']) + layout.addRow(name, joint_widget) + # NOTE: Using partial instead of a lambda because lambdas + # "will not evaluate/look up the argument values before it is + # effectively called, breaking situations like using a loop + # variable inside it" + from functools import partial + par = partial(self._update_single_cmd_cb, name=name) + joint_widget.valueChanged.connect(par) + except: + # TODO: Can we do better than swallow the exception? + from sys import exc_info + print('Unexpected error:', exc_info()[0]) + + # Enter monitor mode (sending commands disabled) + self._on_jtc_enabled(False) + + # Setup ROS interfaces + jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) + state_topic = jtc_ns + '/state' + cmd_topic = jtc_ns + '/joint_trajectory' + self._state_sub = self._node.create_subscription(JointTrajectoryControllerState, + state_topic, + self._state_cb, + 1) + self._cmd_pub = self._node.create_publisher(JointTrajectory, + cmd_topic, + 1) + + self.jointStateChanged.connect(self._on_joint_state_change) + + self._executor = rclpy.executors.SingleThreadedExecutor() + self._executor.add_node(self._node) + self._executor_thread = threading.Thread(target=self._executor.spin, daemon=True) + self._executor_thread.start() + + def _unload_jtc(self): + # Stop updating the joint positions + try: + self.jointStateChanged.disconnect(self._on_joint_state_change) + except: + pass + + # Reset ROS interfaces + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + # Clear joint widgets + # NOTE: Implementation is a workaround for: + # https://bugreports.qt-project.org/browse/QTBUG-15990 :( + + layout = self._widget.joint_group.layout() + if layout is not None: + while layout.count(): + layout.takeAt(0).widget().deleteLater() + # Delete existing layout by reparenting to temporary + QWidget().setLayout(layout) + self._widget.joint_group.setLayout(QFormLayout()) + + # Reset joint data + self._joint_names = [] + self._joint_pos = {} + + # Enforce monitor mode (sending commands disabled) + self._widget.enable_button.setChecked(False) + + def _running_jtc_info(self): + from .utils import filter_by_type, filter_by_state + + controller_list = self._list_controllers() + jtc_list = filter_by_type(controller_list, + 'JointTrajectoryController', + match_substring=True) + running_jtc_list = filter_by_state(jtc_list, 'active') + return running_jtc_list + + def _unregister_cmd_pub(self): + if self._cmd_pub is not None: + self._node.destroy_publisher(self._cmd_pub) + self._cmd_pub = None + + def _unregister_state_sub(self): + if self._state_sub is not None: + self._node.destroy_subscription(self._state_sub) + self._state_sub = None + + def _unregister_executor(self): + if self._executor is not None: + self._executor.shutdown() + self._executor_thread.join() + self._executor = None + + def _state_cb(self, msg): + actual_pos = {} + for i in range(len(msg.joint_names)): + joint_name = msg.joint_names[i] + joint_pos = msg.actual.positions[i] + actual_pos[joint_name] = joint_pos + self.jointStateChanged.emit(actual_pos) + + def _update_single_cmd_cb(self, val, name): + self._joint_pos[name]['command'] = val + + def _update_cmd_cb(self): + dur = [] + traj = JointTrajectory() + traj.joint_names = self._joint_names + point = JointTrajectoryPoint() + for name in traj.joint_names: + pos = self._joint_pos[name]['position'] + cmd = pos + try: + cmd = self._joint_pos[name]['command'] + except (KeyError): + pass + max_vel = self._robot_joint_limits[name]['max_velocity'] + dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) + point.positions.append(cmd) + duration = rclpy.duration.Duration(seconds=(max(dur) / self._speed_scale)) + point.time_from_start = duration.to_msg() + traj.points.append(point) + + self._cmd_pub.publish(traj) + + def _update_joint_widgets(self): + joint_widgets = self._joint_widgets() + for id in range(len(joint_widgets)): + joint_name = self._joint_names[id] + try: + joint_pos = self._joint_pos[joint_name]['position'] + joint_widgets[id].setValue(joint_pos) + except (KeyError): + pass # Can happen when first connected to controller + + def _joint_widgets(self): # TODO: Cache instead of compute every time? + widgets = [] + layout = self._widget.joint_group.layout() + for row_id in range(layout.rowCount()): + widgets.append(layout.itemAt(row_id, + QFormLayout.FieldRole).widget()) + return widgets + + +def _jtc_joint_names(jtc_info): + # NOTE: We assume that there is at least one hardware interface that + # claims resources (there should be), and the resource list is fetched + # from the first available interface + + joint_names = [] + for interface in jtc_info.claimed_interfaces: + name = interface.split("/")[0] + joint_names.append(name) + + return joint_names + + +def _resolve_controller_ns(cm_ns, controller_name): + """ + Resolve a controller's namespace from that of the controller manager. + Controllers are assumed to live one level above the controller + manager, e.g. + + >>> _resolve_controller_ns('/path/to/controller_manager', 'foo') + '/path/to/foo' + + In the particular case in which the controller manager is not + namespaced, the controller is assumed to live in the root namespace + + >>> _resolve_controller_ns('/', 'foo') + '/foo' + >>> _resolve_controller_ns('', 'foo') + '/foo' + + @param cm_ns Controller manager namespace (can be an empty string) + @type cm_ns str + @param controller_name Controller name (non-empty string) + @type controller_name str + @return Controller namespace + @rtype str + """ + assert(controller_name) + ns = cm_ns.rsplit('/', 1)[0] + if ns != '/': + ns += '/' + ns += controller_name + return ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py new file mode 100755 index 0000000000..d0eb860fc5 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller')) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py new file mode 100644 index 0000000000..64e9565cd6 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +def update_combo(combo, new_vals): + """ + Update the contents of a combo box with a set of new values. + + If the previously selected element is still present in the new values, it + will remain as active selection, even if its index has changed. This will + not trigger any signal. + + If the previously selected element is no longer present in the new values, + the combo will unset its selection. This will trigger signals for changed + element. + """ + selected_val = combo.currentText() + old_vals = [combo.itemText(i) for i in range(combo.count())] + + # Check if combo items changed + if not _is_permutation(old_vals, new_vals): + # Determine if selected value is in the new list + selected_id = -1 + try: + selected_id = new_vals.index(selected_val) + except (ValueError): + combo.setCurrentIndex(-1) + + # Re-populate items + combo.blockSignals(True) # No need to notify these changes + combo.clear() + combo.insertItems(0, new_vals) + combo.setCurrentIndex(selected_id) # Restore selection + combo.blockSignals(False) + + +def _is_permutation(a, b): + """ + @type a [] + @type b [] + @return True if C{a} is a permutation of C{b}, false otherwise + @rtype bool + """ + return len(a) == len(b) and sorted(a) == sorted(b) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py new file mode 100644 index 0000000000..e2b6ffa438 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# NOTE: The Python API contained in this file is considered UNSTABLE and +# subject to change. +# No backwards compatibility guarrantees are provided at this moment. + + +import rclpy +from controller_manager_msgs.srv import ListControllers + +# Names of controller manager services, and their respective types +_LIST_CONTROLLERS_STR = 'list_controllers' +_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/srv/ListControllers' +_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' +_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/srv/ListControllerTypes' +_LOAD_CONTROLLER_STR = 'load_controller' +_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/LoadController' +_UNLOAD_CONTROLLER_STR = 'unload_controller' +_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/UnloadController' +_SWITCH_CONTROLLER_STR = 'switch_controller' +_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/srv/SwitchController' +_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' +_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/srv/' + 'ReloadControllerLibraries') + +# Map from service names to their respective type +cm_services = { + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE +} + + +def get_controller_managers(namespace='/', initial_guess=None): + """ + Get list of active controller manager namespaces. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @param initial_guess: Initial guess of the active controller managers. + Typically c{initial_guess} is the output of a previous call to this method, + and is useful when periodically checking for changes in the list of + active controller managers. + Elements in this list will go through a lazy validity check (as opposed to + a full name+type API verification), so providing a good estimate can + significantly reduce the number of ROS master queries incurred by this + method. + @type initial_guess: [str] + @return: Sorted list of active controller manager namespaces. + @rtype: [str] + """ + ns_list = [] + if initial_guess is not None: + ns_list = initial_guess[:] # force copy + + # Get list of (potential) currently running controller managers + node = rclpy.node.Node("get_controller_managers_node") + ns_list_curr = _sloppy_get_controller_managers(node, namespace) + + # Update initial guess: + # 1. Remove entries not found in current list + # 2. Add new untracked controller managers + ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] + ns_list += [ns for ns in ns_list_curr + if ns not in ns_list and is_controller_manager(node, ns)] + + return sorted(ns_list) + + +def is_controller_manager(node, namespace): + """ + Check if the input namespace exposes the controller_manager ROS interface. + + This method has the overhead of several ROS master queries + (one per ROS API member). + + @param namespace: Namespace to check + @type namespace: str + @return: True if namespace exposes the controller_manager ROS interface + @rtype: bool + """ + cm_ns = namespace + if not cm_ns or cm_ns[-1] != '/': + cm_ns += '/' + for srv_name in cm_services.keys(): + if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): + return False + return True + + +def _sloppy_get_controller_managers(node, namespace): + """ + Get list of I{potential} active controller manager namespaces. + + The method name is prepended with I{sloppy}, and the returned list contains + I{potential} active controller managers because it does not perform a + thorough check of the expected ROS API. + It rather tries to minimize the number of ROS master queries. + + This method has the overhead of one ROS master query. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @return: List of I{potential} active controller manager namespaces. + @rtype: [str] + """ + # refresh the list of controller managers we can find + srv_list = node.get_service_names_and_types() + + ns_list = [] + for srv_info in srv_list: + match_str = '/' + _LIST_CONTROLLERS_STR + # First element of srv_name is the service name + if match_str in srv_info[0]: + ns = srv_info[0].split(match_str)[0] + if ns == '': + # controller manager services live in root namespace + # unlikely, but still possible + ns = '/' + ns_list.append(ns) + return ns_list + + +def _srv_exists(node, srv_name, srv_type): + """ + Check if a ROS service of specific name and type exists. + + This method has the overhead of one ROS master query. + + @param srv_name: Fully qualified service name + @type srv_name: str + @param srv_type: Service type + @type srv_type: str + """ + if not srv_name or not srv_type: + return False + + srv_list = node.get_service_names_and_types() + srv_info = [item for item in srv_list if item[0] == srv_name] + srv_obtained_type = srv_info[0][1][0] + return srv_type == srv_obtained_type + + +############################################################################### +# +# Convenience classes for querying controller managers and controllers +# +############################################################################### + +class ControllerManagerLister: + """ + Convenience functor for querying the list of active controller managers. + + Useful when frequently updating the list, as it internally performs + some optimizations that reduce the number of interactions with the + ROS master. + + Example usage: + >>> list_cm = ControllerManagerLister() + >>> print(list_cm()) + """ + + def __init__(self, namespace='/'): + self._ns = namespace + self._cm_list = [] + + def __call__(self): + """Get list of running controller managers.""" + self._cm_list = get_controller_managers(self._ns, self._cm_list) + return self._cm_list + + +class ControllerLister: + """ + Convenience functor for querying loaded controller data. + + The output of calling this functor can be used as input to the different + controller filtering functions available in this module. + + Example usage. Get I{running} controllers of type C{bar_base/bar}: + >>> list_controllers = ControllerLister('foo_robot/controller_manager') + >>> all_ctrl = list_controllers() + >>> running_ctrl = filter_by_state(all_ctrl, 'running') + >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') + """ + + def __init__(self, namespace='/controller_manager'): + """ + @param namespace Namespace of controller manager to monitor. + + @type namespace str + """ + self._node = rclpy.node.Node("controller_lister") + self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv_client = self._create_client() + + """ + @return: Controller list. + @rtype: [controller_manager_msgs/ControllerState] + """ + + def __call__(self): + controller_list = self._srv_client.call_async(ListControllers.Request()) + rclpy.spin_until_future_complete(self._node, controller_list) + return controller_list.result().controller + + def _create_client(self): + return self._node.create_client(ListControllers, self._srv_name) + +############################################################################### +# +# Convenience methods for filtering controller state information +# +############################################################################### + + +def filter_by_name(ctrl_list, ctrl_name, match_substring=False): + """ + Filter controller state list by controller name. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_name: Controller name + @type ctrl_name: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified name + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + + +def filter_by_type(ctrl_list, ctrl_type, match_substring=False): + """ + Filter controller state list by controller type. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_type: Controller type + @type ctrl_type: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified type + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + + +def filter_by_state(ctrl_list, ctrl_state, match_substring=False): + """ + Filter controller state list by controller state. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_state: Controller state + @type ctrl_state: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified state + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + + +def filter_by_hardware_interface(ctrl_list, + hardware_interface, + match_substring=False): + """ + Filter controller state list by controller hardware interface. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param hardware_interface: Controller hardware interface + @type hardware_interface: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if match_substring: + if hardware_interface in resource_set.hardware_interface: + list_out.append(ctrl) + break + else: + if resource_set.hardware_interface == hardware_interface: + list_out.append(ctrl) + break + return list_out + + +def filter_by_resources(ctrl_list, + resources, + hardware_interface=None, + match_any=False): + """ + Filter controller state list by claimed resources. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param resources: Controller resources + @type resources: [str] + @param hardware_interface Controller hardware interface where to look for + resources. If specified, the requested resources will only be searched for + in this interface. If unspecified, all controller hardware interfaces will + be searched for; i.e., if a controller claims resources from multiple + interfaces, the method will succeed if _any_ interface contains the + requested resources (any or all, depending on the value of C{match_any}). + Specifying this parameter allows finer control over determining which + interfaces claim specific resources. + @param match_any: If set to False, all elements in C{resources} must + be claimed by the interface specified in C{hardware_interface} (or _any_ + interface, if C{hardware_interface} is unspecified) for a positive match. + Note that a controller's resources can contain additional entries than + those in C{resources}). + If set to True, at least one element in C{resources} must be claimed by + the interface specified in C{hardware_interface} (or _any_ interface, if + C{hardware_interface} is unspecified) for a positive match. + @type match_any: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if (hardware_interface is None or + hardware_interface == resource_set.hardware_interface): + for res in resources: + add_ctrl = not match_any # Initial flag value + if res in resource_set.resources: + if match_any: # One hit: enough to accept controller + add_ctrl = True + break + else: + if not match_any: # One miss: enough to discard controller + add_ctrl = False + break + if add_ctrl: + list_out.append(ctrl) + break + return list_out + + +def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): + """Filter input list by the value of its elements' attributes.""" + list_out = [] + for val in list_in: + if match_substring: + if attr_val in getattr(val, attr_name): + list_out.append(val) + else: + if getattr(val, attr_name) == attr_val: + list_out.append(val) + return list_out + +############################################################################### +# +# Convenience methods for finding potential controller configurations +# +############################################################################### + +# def get_rosparam_controller_names(namespace='/'): +# """ +# Get list of ROS parameter names that potentially represent a controller +# configuration. + +# Example usage: +# - Assume the following parameters exist in the ROS parameter: +# server: +# - C{/foo/type} +# - C{/xxx/type/xxx} +# - C{/ns/bar/type} +# - C{/ns/yyy/type/yyy} +# - The potential controllers found by this method are: + +# >>> names = get_rosparam_controller_names() # returns ['foo'] +# >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + +# @param namespace: Namespace where to look for controllers. +# @type namespace: str +# @return: Sorted list of ROS parameter names. +# @rtype: [str] +# """ +# import rosparam +# list_out = [] +# all_params = rosparam.list_params(namespace) +# for param in all_params: +# # Remove namespace from parameter string +# if not namespace or namespace[-1] != '/': +# namespace += '/' +# param_no_ns = param.split(namespace, 1)[1] + +# # Check if parameter corresponds to a controller configuration +# param_split = param_no_ns.split('/') +# if (len(param_split) == 2 and param_split[1] == 'type'): +# list_out.append(param_split[0]) # It does! +# return sorted(list_out) diff --git a/rqt_joint_trajectory_controller/setup.cfg b/rqt_joint_trajectory_controller/setup.cfg new file mode 100644 index 0000000000..4c0b982e25 --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/rqt_joint_trajectory_controller +[install] +install-scripts=$base/lib/rqt_joint_trajectory_controller diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py new file mode 100644 index 0000000000..584655963d --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +from setuptools import setup +from glob import glob + +package_name = "rqt_joint_trajectory_controller" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/resource", glob("resource/*.*")), + ("share/" + package_name, ["plugin.xml"]), + + ], + install_requires=["setuptools"], + zip_safe=True, + author="Noel Jimenez Garcia", + author_email="noel.jimenez@pal-robotics.com", + maintainer="Bence Magyar", + maintainer_email="bence.magyar.robotics@gmail.com", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "rqt_joint_trajectory_controller = \ + rqt_joint_trajectory_controller.rqt_joint_trajectory_controller:main", + ], + }, +) From abdbae9ab439f92d185ce89ecc1bdbc6b73ea1b2 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 Jul 2022 01:20:09 -0600 Subject: [PATCH 118/524] Formatting changes from pre-commit (#400) Signed-off-by: Tyler Weaver --- diff_drive_controller/src/diff_drive_controller.cpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 2 +- .../gripper_controllers/gripper_action_controller_impl.hpp | 2 +- .../rqt_joint_trajectory_controller/double_editor.py | 2 +- .../joint_trajectory_controller.py | 7 +++---- .../rqt_joint_trajectory_controller/utils.py | 2 +- 6 files changed, 9 insertions(+), 10 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index aec603ad22..48ebdc9fb7 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -77,8 +77,8 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - publish_limited_velocity_ = auto_declare("publish_limited_velocity", - publish_limited_velocity_); + publish_limited_velocity_ = + auto_declare("publish_limited_velocity", publish_limited_velocity_); auto_declare("velocity_rolling_window_size", 10); use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 98ccbbf5d1..d5999002f4 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -162,7 +162,7 @@ class GripperActionController : public controller_interface::ControllerInterface stall_velocity_threshold_; ///< Stall related parameters double default_max_effort_; ///< Max allowed effort double goal_tolerance_; - bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful + bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful /** * \brief Check for success and publish appropriate result and feedback. diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index f1d270b52f..0b5f791e78 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -181,7 +181,7 @@ void GripperActionController::check_for_success( pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; - if(allow_stalling_) + if (allow_stalling_) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); rt_active_goal_->setSucceeded(pre_alloc_result_); diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py index 23a1f2c806..3fdde9daf9 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -49,7 +49,7 @@ class DoubleEditor(QWidget): valueChanged = Signal(float) def __init__(self, min_val, max_val): - super(DoubleEditor, self).__init__() + super().__init__() # Preconditions assert min_val < max_val diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index f3c98f9fe0..7cb9ea5346 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -14,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from __future__ import print_function import os import rclpy import threading @@ -34,7 +33,7 @@ from .update_combo import update_combo # TODO: -# - Better UI suppor for continuous joints (see DoubleEditor TODO) +# - Better UI support for continuous joints (see DoubleEditor TODO) # - Can we load controller joints faster?, it's currently pretty slow # - If URDF is reloaded, allow to reset the whole plugin? # - Allow to configure: @@ -99,7 +98,7 @@ class JointTrajectoryController(Plugin): jointStateChanged = Signal([dict]) def __init__(self, context): - super(JointTrajectoryController, self).__init__(context) + super().__init__(context) self.setObjectName('JointTrajectoryController') self._node = rclpy.node.Node("rqt_joint_trajectory_controller") self._executor = None @@ -204,7 +203,7 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) - # Resore last session's controller, if running + # Restore last session's controller, if running self._update_jtc_list() jtc_name = instance_settings.value('jtc_name') jtc_combo = self._widget.jtc_combo diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index e2b6ffa438..a84eb2a5f8 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -16,7 +16,7 @@ # NOTE: The Python API contained in this file is considered UNSTABLE and # subject to change. -# No backwards compatibility guarrantees are provided at this moment. +# No backwards compatibility guarantees are provided at this moment. import rclpy From a6937c573ab71c082340bfeccac9320c8014ba9c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 30 Jul 2022 11:08:20 +0100 Subject: [PATCH 119/524] Make JTC callbacks methods with clear names (#397) #abi-breaking MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../joint_trajectory_controller.hpp | 10 ++-- .../src/joint_trajectory_controller.cpp | 52 ++++++++----------- 2 files changed, 30 insertions(+), 32 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 13e5e3ff5f..4c2f612112 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 @@ -205,15 +205,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + // callback for topic interface + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void topic_callback(const std::shared_ptr msg); + // callbacks for action_server_ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_callback( + rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse cancel_callback( + rclcpp_action::CancelResponse goal_cancelled_callback( const std::shared_ptr> goal_handle); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void feedback_setup_callback( + void goal_accepted_callback( std::shared_ptr> goal_handle); // fill trajectory_msg so it matches joints controlled by this controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 235681b069..e04f66754c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -697,30 +697,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( allow_integration_in_goal_trajectories_ = get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); - // subscriber callback - // non realtime - // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) -> void { - if (!validate_trajectory_msg(*msg)) - { - return; - } - - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) - { - add_new_trajectory_msg(msg); - } - }; - - // TODO(karsten1987): create subscriber with subscription deactivated joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - - // TODO(karsten1987): no lifecycle for subscriber yet - // joint_command_subscriber_->on_activate(); + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); @@ -774,9 +754,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), std::string(get_node()->get_name()) + "/follow_joint_trajectory", - std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), - std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); + std::bind(&JointTrajectoryController::goal_received_callback, this, _1, _2), + std::bind(&JointTrajectoryController::goal_cancelled_callback, this, _1), + std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); @@ -857,7 +837,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } - // TODO(karsten1987): activate subscriptions of subscriber return CallbackReturn::SUCCESS; } @@ -988,7 +967,22 @@ void JointTrajectoryController::publish_state( } } -rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( +void JointTrajectoryController::topic_callback( + const std::shared_ptr msg) +{ + if (!validate_trajectory_msg(*msg)) + { + return; + } + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) + { + add_new_trajectory_msg(msg); + } +}; + +rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); @@ -1010,7 +1004,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( +rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); @@ -1034,7 +1028,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( return rclcpp_action::CancelResponse::ACCEPT; } -void JointTrajectoryController::feedback_setup_callback( +void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { // Update new trajectory From 2ee11f5dd65bb0b8f018223c4f3e6700145e5a33 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:44:55 +0100 Subject: [PATCH 120/524] Bring version up to date --- rqt_joint_trajectory_controller/package.xml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 343f733339..a55bcf6a28 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,17 +4,13 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.5.0 + 2.9.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar - Mathias Lüdtke - Enrique Fernandez Apache-2.0 - http://wiki.ros.org/rqt_joint_trajectory_controller - Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia From d1fafffecd2d4a9f88e8638def8431d3d2a343b2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:46:46 +0100 Subject: [PATCH 121/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 6 ++ effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 3 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 6 ++ imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 6 ++ position_controllers/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 84 +++++++++++++++++++ velocity_controllers/CHANGELOG.rst | 3 + 13 files changed, 129 insertions(+) create mode 100644 rqt_joint_trajectory_controller/CHANGELOG.rst diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 54d3e19561..15853cbebc 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) +* Contributors: Andy Zelenak, Tyler Weaver + 2.9.0 (2022-07-14) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d642cba146..b75d868049 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4c6cb3cf70..1bb0bc517f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index ee4131d576..4d79367ca8 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5c9c2bcc8e..6d100fcd90 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) +* Contributors: Andy Zelenak, Tyler Weaver + 2.9.0 (2022-07-14) ------------------ * Allow gripper stalling when moving to goal (`#355 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 13014b5a62..bf35f55674 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 862c663a5d..5efdc7f325 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ca856cdc28..9b5d12042e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking +* Use system time in all tests to avoid error with different time sources. (`#334 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.9.0 (2022-07-14) ------------------ * Add option to skip interpolation in the joint trajectory controller (`#374 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ec225f9c9f..444440d3f2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0aa1a78d34..1f1d8ffc17 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b30e88623f..677f03c8b6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst new file mode 100644 index 0000000000..8c462f5e51 --- /dev/null +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -0,0 +1,84 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_joint_trajectory_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* port rqt_joint_trajectory_controller to ros2 (`#356 `_) +* Contributors: Bence Magyar, Noel Jiménez García, Tyler Weaver + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 24e4c10e5a..40e6d0619f 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ From a37fffec068d6dc3600af5f0bbd896ad590822b0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:47:11 +0100 Subject: [PATCH 122/524] 2.10.0 --- 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 +- 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 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 28 files changed, 41 insertions(+), 41 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 15853cbebc..f582845fe7 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) * Contributors: Andy Zelenak, Tyler Weaver diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index df90159f64..ae2696d998 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.9.0 + 2.10.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b75d868049..30863470a1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d766cdbef3..0e415ba867 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 1bb0bc517f..790834ea9e 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index d5b3e9a3ea..c857ab92ab 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.9.0 + 2.10.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4d79367ca8..16fac9b49f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index d9d60c95cd..3c81a32680 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6d100fcd90..98df8e9bd7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) * Contributors: Andy Zelenak, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 27586a3e97..42ba2dc7f6 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.9.0 + 2.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index bf35f55674..acd580d511 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6a4c01c3c5..52ba10b381 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.9.0 + 2.10.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5efdc7f325..58295ee22b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9b546b81a3..0048dc4c2b 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.9.0 + 2.10.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9b5d12042e..f96bd2c9b4 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking * Use system time in all tests to avoid error with different time sources. (`#334 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 047e778160..02b27cdf83 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.9.0 + 2.10.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 444440d3f2..66cb16f92a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index aa29cc5ea5..8a2fee3598 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1f1d8ffc17..adf3ff3538 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0f6f2b31d8..8d1cca6666 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.9.0 + 2.10.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 677f03c8b6..c7b22b0183 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c6c5a6b457..924e7b0bca 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.9.0 + 2.10.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e10da150bb..4c8fba2fab 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.9.0", + version="2.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 8c462f5e51..3607695579 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * port rqt_joint_trajectory_controller to ros2 (`#356 `_) * Contributors: Bence Magyar, Noel Jiménez García, Tyler Weaver diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index a55bcf6a28..62e703bc3b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.9.0 + 2.10.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 584655963d..ab2db18405 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="0.0.1", + version="2.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 40e6d0619f..6bc28fc062 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 58d2f7f81b..fac41cadbf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 4138b734c3c1389fd715408ead3d24485d3e4c25 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Mon, 1 Aug 2022 23:36:27 -0700 Subject: [PATCH 123/524] Use explicit type in joint_state_broadcaster test (#403) This use of `auto` is causing a static assert on RHEL. Explicitly specifying the type seems to resolve the failure and allow the test to be compiled. --- joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index ecccaa4b22..7f95e232e2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -683,7 +683,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); const size_t NUM_JOINTS = 3; - const auto INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; + const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message From 279ba38aa56168d4e6eaf698b177f1848944cf00 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 3 Aug 2022 19:00:11 +0200 Subject: [PATCH 124/524] Tricycle controller (#345) --- ros2_controllers/package.xml | 1 + tricycle_controller/CMakeLists.txt | 118 ++++ .../config/tricycle_drive_controller.yaml | 43 ++ tricycle_controller/doc/userdoc.rst | 24 + .../include/tricycle_controller/odometry.hpp | 75 ++ .../tricycle_controller/steering_limiter.hpp | 98 +++ .../tricycle_controller/traction_limiter.hpp | 104 +++ .../tricycle_controller.hpp | 185 +++++ .../tricycle_controller/visibility_control.h | 53 ++ tricycle_controller/package.xml | 35 + tricycle_controller/src/odometry.cpp | 121 ++++ tricycle_controller/src/steering_limiter.cpp | 116 +++ tricycle_controller/src/traction_limiter.cpp | 142 ++++ .../src/tricycle_controller.cpp | 668 ++++++++++++++++++ .../test/config/test_tricycle_controller.yaml | 20 + .../test/test_load_tricycle_controller.cpp | 43 ++ .../test/test_tricycle_controller.cpp | 350 +++++++++ tricycle_controller/tricycle_controller.xml | 7 + 18 files changed, 2203 insertions(+) create mode 100644 tricycle_controller/CMakeLists.txt create mode 100644 tricycle_controller/config/tricycle_drive_controller.yaml create mode 100644 tricycle_controller/doc/userdoc.rst create mode 100644 tricycle_controller/include/tricycle_controller/odometry.hpp create mode 100644 tricycle_controller/include/tricycle_controller/steering_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/traction_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/tricycle_controller.hpp create mode 100644 tricycle_controller/include/tricycle_controller/visibility_control.h create mode 100644 tricycle_controller/package.xml create mode 100644 tricycle_controller/src/odometry.cpp create mode 100644 tricycle_controller/src/steering_limiter.cpp create mode 100644 tricycle_controller/src/traction_limiter.cpp create mode 100644 tricycle_controller/src/tricycle_controller.cpp create mode 100644 tricycle_controller/test/config/test_tricycle_controller.yaml create mode 100644 tricycle_controller/test/test_load_tricycle_controller.cpp create mode 100644 tricycle_controller/test/test_tricycle_controller.cpp create mode 100644 tricycle_controller/tricycle_controller.xml diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 8d1cca6666..aebcf25ba6 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -19,6 +19,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + tricycle_controller velocity_controllers diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt new file mode 100644 index 0000000000..c499bca983 --- /dev/null +++ b/tricycle_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.8) +project(tricycle_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) + +add_library( + ${PROJECT_NAME} + SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) + +ament_target_dependencies( + ${PROJECT_NAME} + ackermann_msgs + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) +install( + DIRECTORY + include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_tricycle_controller + test/test_tricycle_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + target_include_directories(test_tricycle_controller PRIVATE include) + target_link_libraries(test_tricycle_controller + ${PROJECT_NAME} + ) + + ament_target_dependencies(test_tricycle_controller + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ) + + ament_add_gmock( + test_load_tricycle_controller + test/test_load_tricycle_controller.cpp + ) + target_include_directories(test_load_tricycle_controller PRIVATE include) + ament_target_dependencies(test_load_tricycle_controller + controller_manager + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml new file mode 100644 index 0000000000..4eabf615ef --- /dev/null +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -0,0 +1,43 @@ +tricycle_controller: + ros__parameters: + # Model + traction_joint_name: traction_joint # Name of traction joint in URDF + steering_joint_name: steering_joint # Name of steering joint in URDF + wheel_radius: 0.0 # Radius of front wheel + wheelbase: 0.0 # Distance between center of back wheels and front wheel + + # Odometry + odom_frame_id: odom + base_frame_id: base_link + publish_rate: 50.0 # publish rate of odom and tf + open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry + enable_odom_tf: true # If True, publishes odom<-base_link TF + odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom + + # Rate Limiting + traction: # All values should be positive + # min_velocity: 0.0 + # max_velocity: 1000.0 + # min_acceleration: 0.0 + max_acceleration: 5.0 + # min_deceleration: 0.0 + max_deceleration: 8.0 + # min_jerk: 0.0 + # max_jerk: 1000.0 + steering: + min_position: -1.57 + max_position: 1.57 + # min_velocity: 0.0 + max_velocity: 1.0 + # min_acceleration: 0.0 + # max_acceleration: 1000.0 + + # cmd_vel input + cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received + use_stamped_vel: false # Set to True if using TwistStamped. + + # Debug + publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst new file mode 100644 index 0000000000..2271b8c9cf --- /dev/null +++ b/tricycle_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +.. _tricycle_controller_userdoc: + +tricycle_controller +===================== + +Controller for mobile robots with tricycle drive. +Input for control are robot base_link twist commands which are translated to traction and steering +commands for the tricycle drive base. Odometry is computed from hardware feedback and published. + +Velocity commands +----------------- + +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. +Velocities on other components are ignored. + + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Velocity, acceleration and jerk limits + Automatic stop after command timeout \ No newline at end of file diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp new file mode 100644 index 0000000000..4a958a93c6 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -0,0 +1,75 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_ +#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace tricycle_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + bool update(double left_vel, double right_vel, const rclcpp::Duration & dt); + void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheelbase_; + double wheel_radius_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp new file mode 100644 index 0000000000..51ecac4cbd --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -0,0 +1,98 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class SteeringLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_position Minimum position [m] or [rad] + * \param [in] max_position Maximum position [m] or [rad] + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + */ + SteeringLimiter( + double min_position = NAN, double max_position = NAN, + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN); + + /** + * \brief Limit the position, velocity and acceleration + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & p, double p0, double p1, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_position(double & p); + + /** + * \brief Limit the velocity + * \param [in, out] p position [m] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & p, double p0, double dt); + + /** + * \brief Limit the acceleration + * \param [in, out] p Position [m] or [rad] + * \param [in] p0 Previous position [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & p, double p0, double p1, double dt); + +private: + + // Position limits: + double min_position_; + double max_position_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp new file mode 100644 index 0000000000..4c0f297ee8 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -0,0 +1,104 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class TractionLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] + * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + TractionLimiter( + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, + double min_deceleration = NAN, double max_deceleration = NAN, + double min_jerk = NAN, double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] or [rad/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Deceleration limits: + double min_deceleration_; + double max_deceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp new file mode 100644 index 0000000000..c5ba1c62e3 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -0,0 +1,185 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ +#define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_srvs/srv/empty.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tricycle_controller/odometry.hpp" +#include "tricycle_controller/traction_limiter.hpp" +#include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/visibility_control.h" + +namespace tricycle_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TricycleController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::Twist; + using TwistStamped = geometry_msgs::msg::TwistStamped; + using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + +public: + TRICYCLE_CONTROLLER_PUBLIC + TricycleController(); + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + struct TractionHandle + { + std::reference_wrapper velocity_state; + std::reference_wrapper velocity_command; + }; + struct SteeringHandle + { + std::reference_wrapper position_state; + std::reference_wrapper position_command; + }; + + CallbackReturn get_traction( + const std::string & traction_joint_name, std::vector & joint); + CallbackReturn get_steering( + const std::string & steering_joint_name, std::vector & joint); + double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); + std::tuple twist_to_ackermann(double linear_command, double angular_command); + + std::string traction_joint_name_; + std::string steering_joint_name_; + + // HACK: put into vector to avoid initializing structs because they have no default constructors + std::vector traction_joint_; + std::vector steering_joint_; + + struct WheelParams + { + double wheelbase = 0.0; + double radius = 0.0; + } wheel_params_; + + struct OdometryParams + { + bool open_loop = false; + bool enable_odom_tf = false; + bool odom_only_twist = false; // for doing the pose integration in seperate node + std::string base_frame_id = "base_link"; + std::string odom_frame_id = "odom"; + std::array pose_covariance_diagonal; + std::array twist_covariance_diagonal; + } odom_params_; + + bool publish_ackermann_command_ = false; + std::shared_ptr> ackermann_command_publisher_ = nullptr; + std::shared_ptr> + realtime_ackermann_command_publisher_ = nullptr; + + Odometry odometry_; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + 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}; + + rclcpp::Service::SharedPtr reset_odom_service_; + + std::queue previous_commands_; // last two commands + + // speed limiters + TractionLimiter limiter_traction_; + SteeringLimiter limiter_steering_; + + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + bool reset(); + void halt(); +}; +} // namespace tricycle_controller +#endif // TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/tricycle_controller/include/tricycle_controller/visibility_control.h new file mode 100644 index 0000000000..bc9b34898b --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) +#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT +#else +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#else +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml new file mode 100644 index 0000000000..9dec765d60 --- /dev/null +++ b/tricycle_controller/package.xml @@ -0,0 +1,35 @@ + + + + tricycle_controller + 1.0.0 + Controller for a tricycle drive mobile base + Tony Najjar + Apache License 2.0 + Tony Najjar + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + \ No newline at end of file diff --git a/tricycle_controller/src/odometry.cpp b/tricycle_controller/src/odometry.cpp new file mode 100644 index 0000000000..e0e670480a --- /dev/null +++ b/tricycle_controller/src/odometry.cpp @@ -0,0 +1,121 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include "tricycle_controller/odometry.hpp" + +namespace tricycle_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +bool Odometry::update(double Ws, double alpha, const rclcpp::Duration & dt) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double Vs = Ws * wheel_radius_; + double Vx = Vs * std::cos(alpha); + double theta_dot = Vs * std::sin(alpha) / wheelbase_; + + // Integrate odometry: + integrateExact(Vx * dt.seconds(), theta_dot * dt.seconds()); + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(Vx); + angular_accumulator_.accumulate(theta_dot); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + integrateExact(linear * dt.seconds(), angular * dt.seconds()); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + resetAccumulators(); +} + +void Odometry::setWheelParams(double wheelbase, double wheel_radius) +{ + wheelbase_ = wheelbase; + wheel_radius_ = wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp new file mode 100644 index 0000000000..6912144bd0 --- /dev/null +++ b/tricycle_controller/src/steering_limiter.cpp @@ -0,0 +1,116 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/steering_limiter.hpp" + +namespace tricycle_controller +{ +SteeringLimiter::SteeringLimiter( + double min_position, double max_position, double min_velocity, double max_velocity, + double min_acceleration, double max_acceleration) +: min_position_(min_position), + max_position_(max_position), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration) +{ + if (!std::isnan(min_position_) && std::isnan(max_position_)) max_position_ = -min_position_; + if (!std::isnan(max_position_) && std::isnan(min_position_)) min_position_ = -max_position_; + + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } +} + +double SteeringLimiter::limit(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(p, p0, p1, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(p, p0, dt); + if (!std::isnan(min_position_) && !std::isnan(max_position_)) limit_position(p); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_position(double & p) +{ + const double tmp = p; + p = rcppmath::clamp(p, min_position_, max_position_); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_velocity(double & p, double p0, double dt) +{ + const double tmp = p; + + const double dv_min = min_velocity_ * dt; + const double dv_max = max_velocity_ * dt; + + double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + dv *= (p - p0 >= 0 ? 1 : -1); + p = p0 + dv; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + const double dv = p - p0; + const double dp0 = p0 - p1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_acceleration_ * dt2; + const double da_max = max_acceleration_ * dt2; + + double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + da *= (dv - dp0 >= 0 ? 1 : -1); + p = p0 + dp0 + da; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp new file mode 100644 index 0000000000..c2a0d88ab4 --- /dev/null +++ b/tricycle_controller/src/traction_limiter.cpp @@ -0,0 +1,142 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/traction_limiter.hpp" + +namespace tricycle_controller +{ +TractionLimiter::TractionLimiter( + double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, + double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) +: min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_deceleration_(min_deceleration), + max_deceleration_(max_deceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + + if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; + if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } + + if (min_deceleration_ < 0 || max_deceleration_ < 0) + { + throw std::invalid_argument("Deceleration cannot be negative." + error); + } + + if (min_jerk_ < 0 || max_jerk_ < 0) + { + throw std::invalid_argument("Jerk cannot be negative." + error); + } +} + +double TractionLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(v, v0, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + + v *= tmp >= 0 ? 1 : -1; + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + double dv_min; + double dv_max; + if (abs(v) >= abs(v0)) + { + dv_min = min_acceleration_ * dt; + dv_max = max_acceleration_ * dt; + } + else + { + dv_min = min_deceleration_ * dt; + dv_max = max_deceleration_ * dt; + } + double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + dv *= (v - v0 >= 0 ? 1 : -1); + v = v0 + dv; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + da *= (dv - dv0 >= 0 ? 1 : -1); + v = v0 + dv0 + da; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp new file mode 100644 index 0000000000..7cec107a91 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -0,0 +1,668 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tricycle_controller/tricycle_controller.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; +} // namespace + +namespace tricycle_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +TricycleController::TricycleController() : controller_interface::ControllerInterface() {} + +CallbackReturn TricycleController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare("traction_joint_name", std::string()); + auto_declare("steering_joint_name", std::string()); + + auto_declare("wheelbase", wheel_params_.wheelbase); + auto_declare("wheel_radius", wheel_params_.radius); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + auto_declare("odom_only_twist", odom_params_.odom_only_twist); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("publish_ackermann_command", publish_ackermann_command_); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + auto_declare("publish_rate", publish_rate_); + + auto_declare("traction.max_velocity", NAN); + auto_declare("traction.min_velocity", NAN); + auto_declare("traction.max_acceleration", NAN); + auto_declare("traction.min_acceleration", NAN); + auto_declare("traction.max_deceleration", NAN); + auto_declare("traction.min_deceleration", NAN); + auto_declare("traction.max_jerk", NAN); + auto_declare("traction.min_jerk", NAN); + + auto_declare("steering.max_position", NAN); + auto_declare("steering.min_position", NAN); + auto_declare("steering.max_velocity", NAN); + auto_declare("steering.min_velocity", NAN); + auto_declare("steering.max_acceleration", NAN); + auto_declare("steering.min_acceleration", NAN); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration TricycleController::command_interface_configuration() const +{ + InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return command_interfaces_config; +} + +InterfaceConfiguration TricycleController::state_interface_configuration() const +{ + InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return state_interfaces_config; +} + +controller_interface::return_type TricycleController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); + if (last_command_msg == nullptr) + { + RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + // command may be limited further by Limiters, + // without affecting the stored twist command + TwistStamped command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; + double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + + if (odom_params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, period); + } + else + { + if (std::isnan(Ws_read) || std::isnan(alpha_read)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + return controller_interface::return_type::ERROR; + } + odometry_.update(Ws_read, alpha_read, period); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) + { + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + } + + // Compute wheel velocity and angle + auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); + + // Reduce wheel speed until the target angle has been reached + double alpha_delta = abs(alpha_write - alpha_read); + double scale; + if (alpha_delta < M_PI / 6) + { + scale = 1; + } + else if (alpha_delta > M_PI_2) + { + scale = 0.01; + } + else + { + // TODO: find the best function, e.g convex power functions + scale = cos(alpha_delta); + } + Ws_write *= scale; + + auto & last_command = previous_commands_.back(); + auto & second_to_last_command = previous_commands_.front(); + + limiter_traction_.limit( + Ws_write, last_command.speed, second_to_last_command.speed, period.seconds()); + + limiter_steering_.limit( + alpha_write, last_command.steering_angle, second_to_last_command.steering_angle, + period.seconds()); + + previous_commands_.pop(); + AckermannDrive ackermann_command; + ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + ackermann_command.steering_angle = alpha_write; + previous_commands_.emplace(ackermann_command); + + // Publish ackermann command + if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + { + auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; + realtime_ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command_publisher_->unlockAndPublish(); + } + + traction_joint_[0].velocity_command.get().set_value(Ws_write); + steering_joint_[0].position_command.get().set_value(alpha_write); + return controller_interface::return_type::OK; +} + +CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto logger = get_node()->get_logger(); + + // update parameters + traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); + steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); + if (traction_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + if (steering_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + + wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + + odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); + odometry_.setVelocityRollingWindowSize( + get_node()->get_parameter("velocity_rolling_window_size").as_int()); + + odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); + + cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + + try + { + limiter_traction_ = TractionLimiter( + get_node()->get_parameter("traction.min_velocity").as_double(), + get_node()->get_parameter("traction.max_velocity").as_double(), + get_node()->get_parameter("traction.min_acceleration").as_double(), + get_node()->get_parameter("traction.max_acceleration").as_double(), + get_node()->get_parameter("traction.min_deceleration").as_double(), + get_node()->get_parameter("traction.max_deceleration").as_double(), + get_node()->get_parameter("traction.min_jerk").as_double(), + get_node()->get_parameter("traction.max_jerk").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + try + { + limiter_steering_ = SteeringLimiter( + get_node()->get_parameter("steering.min_position").as_double(), + get_node()->get_parameter("steering.max_position").as_double(), + get_node()->get_parameter("steering.min_velocity").as_double(), + get_node()->get_parameter("steering.max_velocity").as_double(), + get_node()->get_parameter("steering.min_acceleration").as_double(), + get_node()->get_parameter("steering.max_acceleration").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring steering limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + if (!reset()) + { + return CallbackReturn::ERROR; + } + + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // Fill last two commands with default constructed commands + const AckermannDrive empty_ackermann_drive; + previous_commands_.emplace(empty_ackermann_drive); + previous_commands_.emplace(empty_ackermann_drive); + + // initialize ackermann command publisher + if (publish_ackermann_command_) + { + ackermann_command_publisher_ = get_node()->create_publisher( + DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_ackermann_command_publisher_ = + std::make_shared>( + ackermann_command_publisher_); + } + + // 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 + { + 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_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(); + }); + } + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_params_.odom_frame_id; + odometry_message.child_frame_id = odom_params_.base_frame_id; + + previous_publish_timestamp_ = get_node()->get_clock()->now(); + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + if (odom_params_.enable_odom_tf) + { + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + } + + // Create odom reset service + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &TricycleController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); + + // Initialize the joints + const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); + const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) + { + return CallbackReturn::ERROR; + } + if (traction_joint_.empty() || steering_joint_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Either steering or traction interfaces are non existent"); + return CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +void TricycleController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + odometry_.resetOdometry(); + RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); +} + +bool TricycleController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + std::queue empty_ackermann_drive; + std::swap(previous_commands_, empty_ackermann_drive); + + traction_joint_.clear(); + steering_joint_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + return true; +} + +CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void TricycleController::halt() +{ + traction_joint_[0].velocity_command.get().set_value(0.0); + steering_joint_[0].position_command.get().set_value(0.0); +} + +CallbackReturn TricycleController::get_traction( + const std::string & traction_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the traction joint instance + joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::get_steering( + const std::string & steering_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the steering joint instance + joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +double TricycleController::convert_trans_rot_vel_to_steering_angle( + double Vx, double theta_dot, double wheelbase) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase / Vx); +} + +std::tuple TricycleController::twist_to_ackermann(double Vx, double theta_dot) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double alpha, Ws; + + if (Vx == 0 && theta_dot != 0) + { // is spin action + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + return std::make_tuple(alpha, Ws); + } + + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); + Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + return std::make_tuple(alpha, Ws); +} + +} // namespace tricycle_controller + +#include +PLUGINLIB_EXPORT_CLASS( + tricycle_controller::TricycleController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml new file mode 100644 index 0000000000..d81d722219 --- /dev/null +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -0,0 +1,20 @@ + +test_tricycle_controller: + ros__parameters: + traction_joint_name: traction_joint + steering_joint_name: steering_joint + wheel_radius: 0.125 + wheelbase: 1.252 + use_stamped_vel: false + enable_odom_tf: false + use_sim_time: false + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + odom_only_twist: true + publish_ackermann_command: true + traction: + max_acceleration: 5.0 + max_deceleration: 8.0 + steering: + max_position: 1.57 # pi/2 + max_velocity: 1.0 \ No newline at end of file diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp new file mode 100644 index 0000000000..088cddbabe --- /dev/null +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -0,0 +1,43 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_tricycle_controller", "tricycle_controller/TricycleController")); + + rclcpp::shutdown(); +} diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp new file mode 100644 index 0000000000..a1f09ddaf8 --- /dev/null +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -0,0 +1,350 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tricycle_controller/tricycle_controller.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +class TestableTricycleController : public tricycle_controller::TricycleController +{ +public: + using TricycleController::TricycleController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get(ret); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ + bool wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(velocity_command_subscriber_); + + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + return true; + } + return false; + } +}; + +class TestTricycleController : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() override + { + controller_ = std::make_unique(); + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResources() + { + std::vector state_ifs; + state_ifs.emplace_back(steering_joint_pos_state_); + state_ifs.emplace_back(traction_joint_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(steering_joint_pos_cmd_); + command_ifs.emplace_back(traction_joint_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + const std::string controller_name = "test_tricycle_controller"; + std::unique_ptr controller_; + + const std::string traction_joint_name = "traction_joint"; + const std::string steering_joint_name = "steering_joint"; + double position_ = 0.1; + double velocity_ = 0.2; + + hardware_interface::StateInterface steering_joint_pos_state_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::StateInterface traction_joint_vel_state_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + hardware_interface::CommandInterface steering_joint_pos_cmd_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::CommandInterface traction_joint_vel_cmd_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestTricycleController, configure_fails_without_parameters) +{ + const auto ret = controller_->init(controller_name); + 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); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, activate_fails_without_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + // We implicitly test that by default position feedback is required + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResources(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, cleanup) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + executor.cancel(); +} + +TEST_F(TestTricycleController, correct_initialization_using_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + assignResources(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // send msg + const double linear = 1.0; + const double angular = 0.0; + publish(linear, angular); + // wait for msg is be published to the system + ASSERT_TRUE(controller_->wait_for_twist(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} diff --git a/tricycle_controller/tricycle_controller.xml b/tricycle_controller/tricycle_controller.xml new file mode 100644 index 0000000000..568b13532b --- /dev/null +++ b/tricycle_controller/tricycle_controller.xml @@ -0,0 +1,7 @@ + + + + The tricycle controller transforms linear and angular velocity messages into signals for steering and traction joints for a tricycle drive robot. + + + From 74a3152604552960cdba3f58b9693db0394f981d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:26:09 +0100 Subject: [PATCH 125/524] Update version and maintainer --- tricycle_controller/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9dec765d60..3864d636e2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,8 +2,9 @@ tricycle_controller - 1.0.0 + 2.10.0 Controller for a tricycle drive mobile base + Bence Magyar Tony Najjar Apache License 2.0 Tony Najjar @@ -32,4 +33,4 @@ ament_cmake - \ No newline at end of file + From 647bd3e0c92a40aa945c943c937387eed33ac7ba Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:28:17 +0100 Subject: [PATCH 126/524] Update changelogs --- 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 | 8 ++++++++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 8 ++++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 54 insertions(+) create mode 100644 tricycle_controller/CHANGELOG.rst diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index f582845fe7..294b73484a 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 30863470a1..284d3cab3e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 790834ea9e..9c1247c5b7 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 16fac9b49f..df23e7c7cf 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 98df8e9bd7..850c0a5d3d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index acd580d511..062782fbbb 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 58295ee22b..1f818f4dd4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use explicit type in joint_state_broadcaster test (`#403 `_) + This use of `auto` is causing a static assert on RHEL. Explicitly + specifying the type seems to resolve the failure and allow the test to + be compiled. +* Contributors: Scott K Logan + 2.10.0 (2022-08-01) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f96bd2c9b4..2b893a5702 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 66cb16f92a..c1fc2069f5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index adf3ff3538..5911417c1d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Tricycle controller (`#345 `_) +* Contributors: Tony Najjar + 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c7b22b0183..f5099c3648 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 3607695579..0afb4ad6f0 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst new file mode 100644 index 0000000000..8eccb72ea5 --- /dev/null +++ b/tricycle_controller/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Tricycle controller (`#345 `_) +* Contributors: Bence Magyar, Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6bc28fc062..74e9d1d78b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- From ca21178966c612b18f0d23aade2e0cf2c7a80790 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:36:55 +0100 Subject: [PATCH 127/524] 2.11.0 --- 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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 294b73484a..5c40544d25 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index ae2696d998..d82a4a2df8 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.10.0 + 2.11.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 284d3cab3e..3d2babf2ed 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0e415ba867..e716631d9e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9c1247c5b7..a3d5156859 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index c857ab92ab..868d83fdea 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.10.0 + 2.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index df23e7c7cf..9b1d48c875 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 3c81a32680..071701c5cd 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 850c0a5d3d..c06969c439 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 42ba2dc7f6..a68306ee4e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.10.0 + 2.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 062782fbbb..82c99bf60f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 52ba10b381..dec5c6b5e4 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.10.0 + 2.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1f818f4dd4..3030632664 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Use explicit type in joint_state_broadcaster test (`#403 `_) This use of `auto` is causing a static assert on RHEL. Explicitly specifying the type seems to resolve the failure and allow the test to diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 0048dc4c2b..12db7f0b2d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.10.0 + 2.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2b893a5702..d435127b2f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 02b27cdf83..66bb5b0999 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.10.0 + 2.11.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 c1fc2069f5..1affcdc4b8 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8a2fee3598..3b7eb27975 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5911417c1d..e53cadd749 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Tricycle controller (`#345 `_) * Contributors: Tony Najjar diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index aebcf25ba6..2298913120 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.10.0 + 2.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f5099c3648..7d319f9034 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 924e7b0bca..a57a484e88 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.10.0 + 2.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 4c8fba2fab..959ad0eb4d 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.10.0", + version="2.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0afb4ad6f0..27bc6030cb 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 62e703bc3b..70b8201345 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.10.0 + 2.11.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index ab2db18405..5f24382cb4 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.10.0", + version="2.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 8eccb72ea5..7c929f4432 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Tricycle controller (`#345 `_) * Contributors: Bence Magyar, Tony Najjar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3864d636e2..409ee6bc40 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.10.0 + 2.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 74e9d1d78b..b9de02a603 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index fac41cadbf..b1d500f1a1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 48ade9c8f5a6a681d59305b6ddb624d753de7cfe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 6 Aug 2022 08:54:37 +0200 Subject: [PATCH 128/524] Small fixes for JTC. (#390) variables in JTC to not clutter other PR with them. fixes of updating parameters on renewed configuration of JTC that were missed --- .../src/joint_trajectory_controller.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e04f66754c..6d81c1d2b1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -68,7 +68,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() state_publish_rate_ = auto_declare("state_publish_rate", 50.0); action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); - std::string interpolation_string = auto_declare( + const std::string interpolation_string = auto_declare( "interpolation_method", interpolation_methods::InterpolationMethodMap.at( interpolation_methods::DEFAULT_INTERPOLATION)); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -697,12 +697,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( allow_integration_in_goal_trajectories_ = get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher + state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); if (state_publish_rate_ > 0.0) { @@ -746,6 +754,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } + action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); From 7d8a26925566a651284087b124b8d56b6854b0a9 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Sun, 7 Aug 2022 16:29:25 -0400 Subject: [PATCH 129/524] [JTC] Hold position if tolerance is violated even during non-active goal (#368) * hold position if tolerance is violated even during non-active goal * rename abort Co-authored-by: Michael Wiznitzer --- .../src/joint_trajectory_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6d81c1d2b1..21501dfb67 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -353,6 +353,11 @@ controller_interface::return_type JointTrajectoryController::update( // to be satisfied or violated within the goal_time_tolerance } } + else if (tolerance_violated_while_moving) + { + set_hold_position(); + RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + } } } From 4d3404f8bf394b2f11dd1b4fd9be4070b4bf5767 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 10 Aug 2022 18:04:03 +0200 Subject: [PATCH 130/524] Adjust CI configuration to ros2_control repository. (#408) * Remove old CI setup. * Adding adjusted CI configurationt to ros2_control repository. * Fixup line for private repositories. * Run format only on PR. * Adjust repos files. --- .github/workflows/README.md | 24 ++++++++++ .github/workflows/ci-coverage-build.yml | 48 +++++++++++++++++++ .github/workflows/ci-format.yml | 6 +-- .github/workflows/ci-ros-lint.yml | 44 ++++++----------- .github/workflows/foxy-abi-compatibility.yml | 26 +++++----- ...y-build.yml => foxy-binary-build-main.yml} | 11 +++-- .../workflows/foxy-binary-build-testing.yml | 26 ++++++++++ .github/workflows/foxy-rhel-binary-build.yml | 33 +++++++++++++ ...ld.yml => foxy-semi-binary-build-main.yml} | 10 ++-- .../foxy-semi-binary-build-testing.yml | 25 ++++++++++ .../workflows/galactic-abi-compatibility.yml | 26 +++++----- ...ild.yml => galactic-binary-build-main.yml} | 11 +++-- .../galactic-binary-build-testing.yml | 26 ++++++++++ .../workflows/galactic-rhel-binary-build.yml | 33 +++++++++++++ ...ml => galactic-semi-binary-build-main.yml} | 10 ++-- .../galactic-semi-binary-build-testing.yml | 25 ++++++++++ .../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 ++++++++++ ...urce-build.yml => humble-source-build.yml} | 12 +++-- .../reusable-industrial-ci-with-cache.yml | 30 ++++-------- .../reusable-ros-tooling-source-build.yml | 28 +++++------ .../workflows/rolling-abi-compatibility.yml | 5 +- ...uild.yml => rolling-binary-build-main.yml} | 11 +++-- .../rolling-binary-build-testing.yml | 26 ++++++++++ .../workflows/rolling-rhel-binary-build.yml | 36 +++++++------- ...yml => rolling-semi-binary-build-main.yml} | 10 ++-- .../rolling-semi-binary-build-testing.yml | 25 ++++++++++ .github/workflows/rolling-source-build.yml | 4 ++ .pre-commit-config.yaml | 41 ++++++++++------ README.md | 11 ++--- ros2_controllers-not-released.foxy.repos | 5 ++ ros2_controllers-not-released.galactic.repos | 5 ++ ros2_controllers-not-released.humble.repos | 6 +++ ros2_controllers-not-released.rolling.repos | 5 ++ ros2_controllers.humble.repos | 13 +++++ ros2_controllers.rolling.repos | 10 ++-- 40 files changed, 658 insertions(+), 164 deletions(-) create mode 100644 .github/workflows/README.md create mode 100644 .github/workflows/ci-coverage-build.yml rename .github/workflows/{foxy-binary-build.yml => foxy-binary-build-main.yml} (60%) create mode 100644 .github/workflows/foxy-binary-build-testing.yml create mode 100644 .github/workflows/foxy-rhel-binary-build.yml rename .github/workflows/{foxy-semi-binary-build.yml => foxy-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/foxy-semi-binary-build-testing.yml rename .github/workflows/{galactic-binary-build.yml => galactic-binary-build-main.yml} (60%) create mode 100644 .github/workflows/galactic-binary-build-testing.yml create mode 100644 .github/workflows/galactic-rhel-binary-build.yml rename .github/workflows/{galactic-semi-binary-build.yml => galactic-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/galactic-semi-binary-build-testing.yml 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 rename .github/workflows/{galactic-source-build.yml => humble-source-build.yml} (56%) rename .github/workflows/{rolling-binary-build.yml => rolling-binary-build-main.yml} (60%) create mode 100644 .github/workflows/rolling-binary-build-testing.yml rename .github/workflows/{rolling-semi-binary-build.yml => rolling-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/rolling-semi-binary-build-testing.yml create mode 100644 ros2_controllers-not-released.humble.repos create mode 100644 ros2_controllers.humble.repos diff --git a/.github/workflows/README.md b/.github/workflows/README.md new file mode 100644 index 0000000000..34bfb4cc03 --- /dev/null +++ b/.github/workflows/README.md @@ -0,0 +1,24 @@ +## 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) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) + + +### 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/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml new file mode 100644 index 0000000000..d9b4ec4494 --- /dev/null +++ b/.github/workflows/ci-coverage-build.yml @@ -0,0 +1,48 @@ +name: Coverage Build +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + +jobs: + coverage: + name: coverage build + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + env: + ROS_DISTRO: rolling + steps: + - uses: ros-tooling/setup-ros@0.3.4 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: actions/checkout@v3 + - uses: ros-tooling/action-ros-ci@0.2.6 + with: + target-ros2-distro: ${{ env.ROS_DISTRO }} + import-token: ${{ secrets.GITHUB_TOKEN }} + # build all packages listed in the meta package + package-name: + diff_drive_controller + + vcs-repo-file-url: | + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ 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.0 + with: + file: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + - uses: actions/upload-artifact@v3.1.0 + with: + name: colcon-logs-coverage-rolling + path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dffa28d1c0..fba9f85911 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -4,11 +4,7 @@ name: Format on: - workflow_dispatch: pull_request: - push: - branches: - - master jobs: pre-commit: @@ -20,7 +16,7 @@ jobs: with: python-version: 3.9.7 - name: Install system hooks - run: sudo apt install -qq clang-format-11 cppcheck + run: sudo apt install -qq clang-format-12 cppcheck - uses: pre-commit/action@v2.0.3 with: extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 8eec08251f..9902dbdaaf 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -9,49 +9,33 @@ jobs: strategy: fail-fast: false matrix: - linter: [copyright, cppcheck, lint_cmake] + linter: [cppcheck, copyright, lint_cmake] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: ${{ matrix.linter }} package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + diff_drive_controller - ament_lint_cpplint: - name: ament_lint_cpplint + + ament_lint_100: + name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: fail-fast: false + matrix: + linter: [cpplint] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: cpplint - arguments: "--filter=-whitespace/newline" + arguments: "--linelength=100 --filter=-whitespace/newline" package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + diff_drive_controller + ros2_controllers diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml index 7f9f1684ef..7ce17effd0 100644 --- a/.github/workflows/foxy-abi-compatibility.yml +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -1,18 +1,20 @@ -name: ABI Compatibility Check +name: Foxy - ABI Compatibility Check on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# abi_check: -# runs-on: ubuntu-latest -# steps: -# - uses: actions/checkout@v3 -# - uses: ros-industrial/industrial_ci@master -# env: -# ROS_DISTRO: foxy -# ROS_REPO: main -# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} -# NOT_TEST_BUILD: true + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: foxy + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build.yml b/.github/workflows/foxy-binary-build-main.yml similarity index 60% rename from .github/workflows/foxy-binary-build.yml rename to .github/workflows/foxy-binary-build-main.yml index bd3050cd37..b306d7e44d 100644 --- a/.github/workflows/foxy-binary-build.yml +++ b/.github/workflows/foxy-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Foxy Binary Build -# Build & test all dependencies from released (binary) packages. +name: Foxy Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -10,12 +14,13 @@ on: - foxy schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_controllers-not-released.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml new file mode 100644 index 0000000000..260751abea --- /dev/null +++ b/.github/workflows/foxy-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Foxy Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + 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: foxy + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-rhel-binary-build.yml b/.github/workflows/foxy-rhel-binary-build.yml new file mode 100644 index 0000000000..8ce90c940f --- /dev/null +++ b/.github/workflows/foxy-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Foxy RHEL Binary Build +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + foxy_rhel_binary: + name: Foxy RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: foxy + container: jaronl/ros:foxy-alma + steps: + - uses: actions/checkout@v3 + 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/foxy-semi-binary-build.yml b/.github/workflows/foxy-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/foxy-semi-binary-build.yml rename to .github/workflows/foxy-semi-binary-build-main.yml index a6fc83fbd5..75c3295124 100644 --- a/.github/workflows/foxy-semi-binary-build.yml +++ b/.github/workflows/foxy-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Foxy Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Foxy Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -10,12 +13,13 @@ on: - foxy schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_controllers.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml new file mode 100644 index 0000000000..f6d663cc2c --- /dev/null +++ b/.github/workflows/foxy-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Foxy Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + 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: foxy + ros_repo: testing + upstream_workspace: ros2_controllers.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml index 8689791128..06a48ef9c7 100644 --- a/.github/workflows/galactic-abi-compatibility.yml +++ b/.github/workflows/galactic-abi-compatibility.yml @@ -1,18 +1,20 @@ -name: ABI Compatibility Check +name: Galactic - ABI Compatibility Check on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# abi_check: -# runs-on: ubuntu-latest -# steps: -# - uses: actions/checkout@v3 -# - uses: ros-industrial/industrial_ci@master -# env: -# ROS_DISTRO: galactic -# ROS_REPO: main -# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} -# NOT_TEST_BUILD: true + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: galactic + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build.yml b/.github/workflows/galactic-binary-build-main.yml similarity index 60% rename from .github/workflows/galactic-binary-build.yml rename to .github/workflows/galactic-binary-build-main.yml index eeac89e8d1..14fe56407c 100644 --- a/.github/workflows/galactic-binary-build.yml +++ b/.github/workflows/galactic-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Galactic Binary Build -# Build & test all dependencies from released (binary) packages.' +name: Galactic Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic @@ -10,12 +14,13 @@ on: - galactic schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_controllers-not-released.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-binary-build-testing.yml b/.github/workflows/galactic-binary-build-testing.yml new file mode 100644 index 0000000000..c4b22a3a7a --- /dev/null +++ b/.github/workflows/galactic-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Galactic Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + 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: galactic + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml new file mode 100644 index 0000000000..2116d5f4d5 --- /dev/null +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Galactic RHEL Binary Build +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + galactic_rhel_binary: + name: Galactic RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: galactic + container: jaronl/ros:galactic-alma + steps: + - uses: actions/checkout@v3 + 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/galactic-semi-binary-build.yml b/.github/workflows/galactic-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/galactic-semi-binary-build.yml rename to .github/workflows/galactic-semi-binary-build-main.yml index f7f2cd0a53..7e498679cb 100644 --- a/.github/workflows/galactic-semi-binary-build.yml +++ b/.github/workflows/galactic-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Galactic Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Galactic Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic @@ -10,12 +13,13 @@ on: - galactic schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_controllers.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-semi-binary-build-testing.yml b/.github/workflows/galactic-semi-binary-build-testing.yml new file mode 100644 index 0000000000..82a74a358b --- /dev/null +++ b/.github/workflows/galactic-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Galactic Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + 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: galactic + ros_repo: testing + upstream_workspace: ros2_controllers.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml new file mode 100644 index 0000000000..1be00cfc76 --- /dev/null +++ b/.github/workflows/humble-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Humble - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - 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..01708cf864 --- /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: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: master diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml new file mode 100644 index 0000000000..73ed0a4859 --- /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: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: master diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml new file mode 100644 index 0000000000..42a700a7db --- /dev/null +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Humble RHEL Binary Build +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: jaronl/ros:humble-alma + steps: + - uses: actions/checkout@v3 + 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..10b1186b79 --- /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: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: master 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..ec73cc6b98 --- /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: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: master diff --git a/.github/workflows/galactic-source-build.yml b/.github/workflows/humble-source-build.yml similarity index 56% rename from .github/workflows/galactic-source-build.yml rename to .github/workflows/humble-source-build.yml index d1c3aa97e8..6ab92814dc 100644 --- a/.github/workflows/galactic-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,11 @@ -name: Galactic Source Build +name: Humble Source Build on: + workflow_dispatch: + branches: + - master push: branches: - - galactic + - master schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -11,5 +14,6 @@ jobs: source: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: - ros_distro: galactic - ref: galactic + ros_distro: humble + ref: master + ros2_repo_branch: master-humble diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 57ea222979..0ff359d091 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -20,8 +20,8 @@ on: required: true type: string ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "all", "main", "testing"' - default: 'all' + description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' + default: 'main' required: false type: string os_code_name: @@ -49,27 +49,23 @@ on: jobs: reusable_industrial_ci_with_cache: - name: ${{ inputs.os_code_name }} ${{ matrix.ROS_REPO }} ${{ inputs.ros_distro }} - strategy: - fail-fast: false - matrix: - ROS_REPO: [ main, testing ] + name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} runs-on: ubuntu-latest env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ matrix.ROS_REPO }}-${{ github.job }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name != 'schedule' }} + if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v3 - name: Checkout ${{ inputs.ref }} on scheduled build - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name == 'schedule' }} + if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v3 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws - if: ${{ ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ ! matrix.env.CCOV }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/target_ws @@ -77,7 +73,6 @@ jobs: restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.CCACHE_DIR }} @@ -85,22 +80,17 @@ jobs: restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} - uses: 'ros-industrial/industrial_ci@master' + - uses: 'ros-industrial/industrial_ci@master' env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ matrix.ROS_REPO }} + ROS_REPO: ${{ inputs.ros_repo }} OS_CODE_NAME: ${{ inputs.os_code_name }} BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ always() && ! matrix.env.CCOV }} run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete sudo rm -rf ${{ env.BASEDIR }}/target_ws/src du -sh ${{ env.BASEDIR }}/target_ws - - name: Is job skipped? - if: ${{ ! (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} - run: | - echo "This job is skpped!" diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 287809991d..2ad549b0f8 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -13,41 +13,37 @@ on: description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' required: true type: string + ros2_repo_branch: + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + default: 'master' + required: false + type: string jobs: reusable_ros_tooling_source_build: - name: ${{ inputs.ros_distro }} ubuntu-20.04 - runs-on: ubuntu-20.04 + name: ${{ inputs.ros_distro }} ubuntu-22.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@0.3.4 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@0.2.6 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package package-name: diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ref }}/ros2.repos + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - uses: actions/upload-artifact@v1 with: - name: colcon-logs-ubuntu-20.04 + name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 1b327f58fb..4589e92e3b 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,5 +1,8 @@ -name: ABI Compatibility Check +name: Rolling - ABI Compatibility Check on: + workflow_dispatch: + branches: + - master pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build-main.yml similarity index 60% rename from .github/workflows/rolling-binary-build.yml rename to .github/workflows/rolling-binary-build-main.yml index 7e0a2e04d6..b51bbabe29 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Rolling Binary Build -# Build & test all dependencies from released (binary) packages. +name: Rolling Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -10,12 +14,13 @@ on: - master schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_controllers-not-released.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml new file mode 100644 index 0000000000..a1db89b8d9 --- /dev/null +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Rolling Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: rolling + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 62197a2db9..e85b439411 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,5 +1,8 @@ name: Rolling RHEL Binary Build on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -12,20 +15,19 @@ on: jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# rolling_rhel_binary: -# name: Rolling RHEL binary build -# runs-on: ubuntu-latest -# env: -# ROS_DISTRO: rolling -# container: jaronl/ros:rolling-alma -# steps: -# - uses: actions/checkout@v3 -# 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 + rolling_rhel_binary: + name: Rolling RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: jaronl/ros:rolling-alma + steps: + - uses: actions/checkout@v3 + 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/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/rolling-semi-binary-build.yml rename to .github/workflows/rolling-semi-binary-build-main.yml index 25666c0b0c..eca9483438 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Rolling Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Rolling Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -10,12 +13,13 @@ on: - master schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml new file mode 100644 index 0000000000..62214adae9 --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Rolling Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + 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: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 0a0ae8de43..1a137a795b 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,5 +1,8 @@ name: Rolling Source Build on: + workflow_dispatch: + branches: + - master push: branches: - master @@ -13,3 +16,4 @@ jobs: with: ros_distro: rolling ref: master + ros2_repo_branch: master-rolling diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ba3dd242ac..fa8d75100e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.3.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,24 +33,29 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.31.1 + rev: v2.37.3 hooks: - - id: pyupgrade + - id: pyupgrade args: [--py36-plus] + - repo: https://github.com/psf/black + rev: 22.6.0 + hooks: + - id: black + args: ["--line-length=99"] + # PEP 257 - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 rev: v0.3.3 hooks: - - id: pep257 - args: - ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - id: pep257 + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 4.0.1 + rev: 5.0.4 hooks: - - id: flake8 - args: ["--ignore=E501"] + - id: flake8 + args: ["--ignore=E501"] # CPP hooks - repo: local @@ -58,10 +63,16 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-11 + entry: clang-format-12 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"] + 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: @@ -108,10 +119,10 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.10.1 + rev: v1.0.0 hooks: - id: doc8 - args: ["--max-line-length=100", "--ignore=D001"] + args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks @@ -128,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ["--write-changes"] - exclude: CHANGELOG\.rst|\.(svg|pyc)$ + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/README.md b/README.md index d7aa74a173..6a59d0054e 100644 --- a/README.md +++ b/README.md @@ -6,11 +6,10 @@ Commonly used and generalized controllers for ros2-control framework that are re ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![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) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/)
[API Reference](https://control.ros.org/galactic/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/)
[API Reference](https://control.ros.org/foxy/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) - +**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) +**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) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types @@ -25,8 +24,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages Uses repos file: `src/$NAME$/$NAME$.repos` -1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. - ## Acknowledgements diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos new file mode 100644 index 0000000000..1b3910e7e7 --- /dev/null +++ b/ros2_controllers-not-released.humble.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos new file mode 100644 index 0000000000..3a667e7d9a --- /dev/null +++ b/ros2_controllers.humble.repos @@ -0,0 +1,13 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: galactic-devel + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 5b2339a137..3a667e7d9a 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -1,17 +1,13 @@ repositories: - ros-controls/ros2_control: + ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git version: master control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel + version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 + version: master From c74e31a29255fa2f66f45874195d50a0f5cba471 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 10 Aug 2022 19:28:15 +0100 Subject: [PATCH 131/524] Reinstate JTC tests (#391) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix correct_initialization_using_parameters * Fix configure_state_ignores_command * Re-enable incorrect_initialization_using_interface_parameters * Re-enable cleanup and activate tests * Port gtest to gmock Co-authored-by: Denis Štogl --- joint_trajectory_controller/CMakeLists.txt | 10 +- .../interpolation_methods.hpp | 3 +- joint_trajectory_controller/package.xml | 2 +- .../src/joint_trajectory_controller.cpp | 58 +- .../src/trajectory.cpp | 3 +- .../test_load_joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory.cpp | 2 +- .../test/test_trajectory_actions.cpp | 20 +- .../test/test_trajectory_controller.cpp | 1808 +++++++++-------- .../test/test_trajectory_controller_utils.hpp | 4 +- 10 files changed, 973 insertions(+), 939 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 793b736a05..5689e589f8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -51,15 +51,15 @@ install(TARGETS ${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gtest(test_trajectory test/test_trajectory.cpp) + ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_include_directories(test_trajectory PRIVATE include) target_link_libraries(test_trajectory ${PROJECT_NAME}) - ament_add_gtest(test_trajectory_controller + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) @@ -71,7 +71,7 @@ if(BUILD_TESTING) ${THIS_PACKAGE_INCLUDE_DEPENDS} ) - ament_add_gtest( + ament_add_gmock( test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp ) @@ -84,7 +84,7 @@ if(BUILD_TESTING) ) # TODO(andyz): Disabled due to flakiness - # ament_add_gtest( + # ament_add_gmock( # test_trajectory_actions # test/test_trajectory_actions.cpp # ) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index 1cebd56f8a..766be6e0f4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -15,10 +15,11 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ -#include #include #include +#include "rclcpp/rclcpp.hpp" + namespace joint_trajectory_controller { static const rclcpp::Logger LOGGER = diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 66bb5b0999..00e6e9f6cb 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -27,7 +27,7 @@ rclcpp_lifecycle trajectory_msgs - ament_cmake_gtest + ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 21501dfb67..5898a6fe95 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -134,7 +134,8 @@ controller_interface::return_type JointTrajectoryController::update( auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) { + const JointTrajectoryPoint & desired) + { // error defined as the difference between current and desired error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]); @@ -162,12 +163,13 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; + [&](auto & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; // current state update state_current_.time_from_start.set__sec(0); @@ -368,12 +370,13 @@ controller_interface::return_type JointTrajectoryController::update( void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; // Assign values from the hardware // Position states always exist @@ -406,17 +409,20 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto bool has_values = true; auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; - auto interface_has_values = [](const auto & joint_interface) { - return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { - return std::isnan(interface.get().get_value()); - }) == joint_interface.end(); + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; // Assign values from the command interfaces as state. Therefore needs check for both. @@ -676,7 +682,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } - auto get_interface_list = [](const std::vector & interface_types) { + auto get_interface_list = [](const std::vector & interface_types) + { std::stringstream ss_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { @@ -1133,7 +1140,8 @@ void JointTrajectoryController::sort_to_local_joint_order( std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); auto remap = [this]( const std::vector & to_remap, - const std::vector & mapping) -> std::vector { + const std::vector & mapping) -> std::vector + { if (to_remap.empty()) { return to_remap; diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index a65329d289..f28703efc9 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -179,7 +179,8 @@ void Trajectory::interpolate_between_points( output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); - auto generate_powers = [](int n, double x, double * powers) { + auto generate_powers = [](int n, double x, double * powers) + { powers[0] = 1.0; for (int i = 1; i <= n; ++i) { diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 7f80badebc..e0dfdc6353 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include "controller_manager/controller_manager.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index cef8ad00dd..93e3560c07 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -17,7 +17,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index cc91071e0c..7128c0bee2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -87,16 +87,18 @@ class TestTrajectoryActions : public TrajectoryControllerTest { setup_controller_hw_ = true; - controller_hw_thread_ = std::thread([&]() { - // controller hardware cycle update loop - auto start_time = rclcpp::Clock().now(); - rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) + controller_hw_thread_ = std::thread( + [&]() { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); - } - }); + // controller hardware cycle update loop + auto start_time = rclcpp::Clock().now(); + rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); + auto end_time = start_time + wait; + while (rclcpp::Clock().now() < end_time) + { + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + } + }); // sometimes doesn't receive calls when we don't sleep std::this_thread::sleep_for(std::chrono::milliseconds(300)); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e4be504717..576f6a30d7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -25,7 +25,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -54,7 +54,6 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" using lifecycle_msgs::msg::State; -using test_trajectory_controllers::TestableJointTrajectoryController; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -62,26 +61,31 @@ bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } -TEST_P(TrajectoryControllerTestParameterized, configure) +TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - const auto future_handle_ = std::async(std::launch::async, spin, &executor); + // const auto future_handle_ = std::async(std::launch::async, spin, &executor); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points, rclcpp::Time()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); // hw position == 0 because controller is not activated EXPECT_EQ(0.0, joint_pos_[0]); @@ -237,13 +241,19 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) executor.add_node(traj_node->get_node_base_interface()); // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points, rclcpp::Time()); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); auto state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -269,7 +279,6 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); - SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -297,18 +306,16 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->wait_for_trajectory(executor); // first update - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); // wait so controller process the second point when deactivated - std::this_thread::sleep_for(FIRST_POINT_TIME); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update( + rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); // deactivated state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - // TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong? - // Come the flackiness here? - const auto allowed_delta = 0.11; // 0.05; + const auto allowed_delta = 0.05; if (traj_controller_->has_position_command_interface()) { EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); @@ -332,907 +339,919 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // cleanup state = traj_controller_->get_node()->cleanup(); - - // update loop receives a new msg and updates accordingly - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - - // check the traj_msg_home_ptr_ initialization code for the standard wait timing - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], allowed_delta); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta); - - state = traj_controller_->get_node()->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - - executor.cancel(); -} - -TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - updateController(); - - // Spin to receive latest state - executor.spin_some(); - auto state = getState(); - - size_t n_joints = joint_names_.size(); - - for (unsigned int i = 0; i < n_joints; ++i) - { - EXPECT_EQ(joint_names_[i], state->joint_names[i]); - } - - // No trajectory by default, no desired state or error - EXPECT_TRUE(state->desired.positions.empty()); - EXPECT_TRUE(state->desired.velocities.empty()); - EXPECT_TRUE(state->desired.accelerations.empty()); - - EXPECT_EQ(n_joints, state->actual.positions.size()); - if ( - std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == - state_interface_types_.end()) - { - EXPECT_TRUE(state->actual.velocities.empty()); - } - else - { - EXPECT_EQ(n_joints, state->actual.velocities.size()); - } - if ( - std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == - state_interface_types_.end()) - { - EXPECT_TRUE(state->actual.accelerations.empty()); - } - else - { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); - } - - EXPECT_TRUE(state->error.positions.empty()); - EXPECT_TRUE(state->error.velocities.empty()); - EXPECT_TRUE(state->error.accelerations.empty()); -} - -void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -{ - rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", static_cast(target_msg_count)); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); - - auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); - - using control_msgs::msg::JointTrajectoryControllerState; - - const int qos_level = 10; - int echo_received_counter = 0; - rclcpp::Subscription::SharedPtr subs = - traj_controller_->get_node()->create_subscription( - controller_name_ + "/state", qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + // TODO(anyone): should the controller even allow calling update() when it is not active? + // update loop receives a new msg and updates accordingly + traj_controller_->update( + rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); - // update for 1second - const auto start_time = rclcpp::Clock().now(); - const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); - const auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) - { - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - } + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); - // We may miss the last message since time allowed is exactly the time needed - EXPECT_NEAR(target_msg_count, echo_received_counter, 1); + // state = traj_controller_->get_node()->configure(); + // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } -/** - * @brief test_state_publish_rate Test that state publish rate matches configure rate - */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -{ - test_state_publish_rate_target(10); -} +// TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); +// updateController(); -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -{ - test_state_publish_rate_target(0); -} +// // Spin to receive latest state +// executor.spin_some(); +// auto state = getState(); -/** - * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from internal controller order - */ -TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - { - trajectory_msgs::msg::JointTrajectory traj_msg; - const std::vector jumbled_joint_names{ - joint_names_[1], joint_names_[2], joint_names_[0]}; - traj_msg.joint_names = jumbled_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(3); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 3.0; - traj_msg.points[0].positions[2] = 1.0; - - if (traj_controller_->has_velocity_command_interface()) - { - traj_msg.points[0].velocities.resize(3); - traj_msg.points[0].velocities[0] = -0.1; - traj_msg.points[0].velocities[1] = -0.1; - traj_msg.points[0].velocities[2] = -0.1; - } - - if (traj_controller_->has_effort_command_interface()) - { - traj_msg.points[0].effort.resize(3); - traj_msg.points[0].effort[0] = -0.1; - traj_msg.points[0].effort[1] = -0.1; - traj_msg.points[0].effort[2] = -0.1; - } - - trajectory_publisher_->publish(traj_msg); - } - - traj_controller_->wait_for_trajectory(executor); - // update for 0.25 seconds - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25)); - - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_GT(0.0, joint_vel_[0]); - EXPECT_GT(0.0, joint_vel_[1]); - EXPECT_GT(0.0, joint_vel_[2]); - } - - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); - } -} +// size_t n_joints = joint_names_.size(); -/** - * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints - */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; - trajectory_msgs::msg::JointTrajectory traj_msg; +// for (unsigned int i = 0; i < n_joints; ++i) +// { +// EXPECT_EQ(joint_names_[i], state->joint_names[i]); +// } - { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; - traj_msg.joint_names = partial_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(2); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 1.0; - traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); - - trajectory_publisher_->publish(traj_msg); - } +// // No trajectory by default, no desired state or error +// EXPECT_TRUE(state->desired.positions.empty()); +// EXPECT_TRUE(state->desired.velocities.empty()); +// EXPECT_TRUE(state->desired.accelerations.empty()); + +// EXPECT_EQ(n_joints, state->actual.positions.size()); +// if ( +// std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == +// state_interface_types_.end()) +// { +// EXPECT_TRUE(state->actual.velocities.empty()); +// } +// else +// { +// EXPECT_EQ(n_joints, state->actual.velocities.size()); +// } +// if ( +// std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == +// state_interface_types_.end()) +// { +// EXPECT_TRUE(state->actual.accelerations.empty()); +// } +// else +// { +// EXPECT_EQ(n_joints, state->actual.accelerations.size()); +// } - traj_controller_->wait_for_trajectory(executor); - // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); +// EXPECT_TRUE(state->error.positions.empty()); +// EXPECT_TRUE(state->error.velocities.empty()); +// EXPECT_TRUE(state->error.accelerations.empty()); +// } - double threshold = 0.001; +// void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +// { +// rclcpp::Parameter state_publish_rate_param( +// "state_publish_rate", static_cast(target_msg_count)); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); + +// auto future_handle = std::async( +// std::launch::async, [&executor]() -> void { executor.spin(); }); + +// using control_msgs::msg::JointTrajectoryControllerState; + +// const int qos_level = 10; +// int echo_received_counter = 0; +// rclcpp::Subscription::SharedPtr subs = +// traj_controller_->get_node()->create_subscription( +// controller_name_ + "/state", qos_level, +// [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + +// // update for 1second +// const auto start_time = rclcpp::Clock().now(); +// const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); +// const auto end_time = start_time + wait; +// while (rclcpp::Clock().now() < end_time) +// { +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// } - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "Joint 3 command should be current position"; - } +// // We may miss the last message since time allowed is exactly the time needed +// EXPECT_NEAR(target_msg_count, echo_received_counter, 1); - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != - command_interface_types_.end()) - { - // estimate the sign of the velocity - // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) - << "Joint 3 velocity should be 0.0 since it's not in the goal"; - } +// executor.cancel(); +// } - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != - command_interface_types_.end()) - { - // estimate the sign of the velocity - // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) - << "Joint 3 effort should be 0.0 since it's not in the goal"; - } - // TODO(anyone): add here ckecks for acceleration commands +// /** +// * @brief test_state_publish_rate Test that state publish rate matches configure rate +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +// { +// test_state_publish_rate_target(10); +// } - executor.cancel(); -} +// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +// { +// test_state_publish_rate_target(0); +// } -/** - * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints without allow_partial_joints_goal - */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +// /** +// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from +// * internal controller order +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// { +// trajectory_msgs::msg::JointTrajectory traj_msg; +// const std::vector jumbled_joint_names{ +// joint_names_[1], joint_names_[2], joint_names_[0]}; +// traj_msg.joint_names = jumbled_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(3); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 3.0; +// traj_msg.points[0].positions[2] = 1.0; + +// if (traj_controller_->has_velocity_command_interface()) +// { +// traj_msg.points[0].velocities.resize(3); +// traj_msg.points[0].velocities[0] = -0.1; +// traj_msg.points[0].velocities[1] = -0.1; +// traj_msg.points[0].velocities[2] = -0.1; +// } + +// if (traj_controller_->has_effort_command_interface()) +// { +// traj_msg.points[0].effort.resize(3); +// traj_msg.points[0].effort[0] = -0.1; +// traj_msg.points[0].effort[1] = -0.1; +// traj_msg.points[0].effort[2] = -0.1; +// } + +// trajectory_publisher_->publish(traj_msg); +// } - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.25 seconds +// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? +// // Currently COMMON_THRESHOLD is adjusted. +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); +// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); +// } - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; - const double initial_joint_vel = 0.0; - const double initial_joint_acc = 0.0; - trajectory_msgs::msg::JointTrajectory traj_msg; +// if (traj_controller_->has_velocity_command_interface()) +// { +// EXPECT_GT(0.0, joint_vel_[0]); +// EXPECT_GT(0.0, joint_vel_[1]); +// EXPECT_GT(0.0, joint_vel_[2]); +// } - { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; - traj_msg.joint_names = partial_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(2); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 1.0; - traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = 2.0; - traj_msg.points[0].velocities[1] = 1.0; - - trajectory_publisher_->publish(traj_msg); - } +// if (traj_controller_->has_effort_command_interface()) +// { +// EXPECT_GT(0.0, joint_eff_[0]); +// EXPECT_GT(0.0, joint_eff_[1]); +// EXPECT_GT(0.0, joint_eff_[2]); +// } +// } - traj_controller_->wait_for_trajectory(executor); - // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = +// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); +// traj_msg.points[0].velocities[1] = +// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + +// trajectory_publisher_->publish(traj_msg); +// } - double threshold = 0.001; - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) - << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) - << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "All joints command should be current position because goal was rejected"; - - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != - command_interface_types_.end()) - { - EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - } +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != - command_interface_types_.end()) - { - EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - } +// double threshold = 0.001; - executor.cancel(); -} +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); +// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "Joint 3 command should be current position"; +// } -/** - * @brief invalid_message Test mismatched joint and reference vector sizes - */ -TEST_P(TrajectoryControllerTestParameterized, invalid_message) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController( - true, {partial_joints_parameters, allow_integration_parameters}, &executor); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // Incompatible joint names - traj_msg = good_traj_msg; - traj_msg.joint_names = {"bad_name"}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few efforts - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].effort = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Non-strictly increasing waypoint times - traj_msg = good_traj_msg; - traj_msg.points.push_back(traj_msg.points.front()); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); +// EXPECT_NEAR(0.0, joint_vel_[2], threshold) +// << "Joint 3 velocity should be 0.0 since it's not in the goal"; +// } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or velocities -/// are accepted -TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) -{ - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - good_traj_msg.points[0].accelerations.resize(1); - good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position and velocity data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // All empty - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - traj_msg.points[0].accelerations.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); +// EXPECT_NEAR(0.0, joint_eff_[2], threshold) +// << "Joint 3 effort should be 0.0 since it's not in the goal"; +// } +// // TODO(anyone): add here ckecks for acceleration commands -/** - * @brief test_trajectory_replace Test replacing an existing trajectory - */ -TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) -{ - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - - subscribeToState(); - - std::vector> points_old{{{2., 3., 4.}}}; - std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; - std::vector> points_partial_new{{1.5}}; - std::vector> points_partial_new_velocities{{0.15}}; - - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; - expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; - expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; - // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); - points_partial_new_velocities[0][0] = - copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); - publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); - - // Replaced trajectory is a mix of previous and current goal - expected_desired.positions[0] = points_partial_new[0][0]; - expected_desired.positions[1] = points_old[0][1]; - expected_desired.positions[2] = points_old[0][2]; - expected_desired.velocities[0] = points_partial_new_velocities[0][0]; - expected_desired.velocities[1] = 0.0; - expected_desired.velocities[2] = 0.0; - expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// executor.cancel(); +// } -/** - * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory - */ -TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - - // TODO(anyone): add expectations for velocities and accelerations - std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new{{{-1., -2., -3.}}}; - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0] trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); - // New trajectory will end before current time - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; - std::cout << "Sending old trajectory" << std::endl; - publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints without allow_partial_joints_goal +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// const double initial_joint_vel = 0.0; +// const double initial_joint_acc = 0.0; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = 2.0; +// traj_msg.points[0].velocities[1] = 1.0; + +// trajectory_publisher_->publish(traj_msg); +// } -TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - - std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - RCLCPP_INFO( - traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); - // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; - expected_desired = expected_actual; - publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// double threshold = 0.001; +// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "All joints command should be current position because goal was rejected"; + +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// } -TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) -{ - SetUpTrajectoryController(); - auto traj_node = traj_controller_->get_node(); - RCLCPP_WARN( - traj_node->get_logger(), - "Test disabled until current_trajectory is taken into account when adding a new trajectory."); - // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 - return; - - // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(traj_node->get_node_base_interface()); - subscribeToState(); - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - traj_node->set_parameter(partial_joints_parameters); - traj_controller_->get_node()->configure(); - traj_controller_->get_node()->activate(); - - std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; - std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; - std::vector> partial_traj{ - {{-1., -2.}, - { - -2., - -4, - }}}; - std::vector> partial_traj_velocities{ - {{-0.1, -0.2}, - { - -0.2, - -0.4, - }}}; - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; - // Send full trajectory - publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); - // Sleep until first waypoint of full trajectory - - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - // Send partial trajectory starting after full trajecotry is complete - RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); - publish( - points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); - // Wait until the end start and end of partial traj - - expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; - expected_desired = expected_actual; - - waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); -} +// if ( +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// } -TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - - // goal setup - std::vector first_goal = {3.3, 4.4, 5.5}; - std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; - std::vector second_goal = {6.6, 8.8, 11.0}; - std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; - double state_from_command_offset = 0.3; +// executor.cancel(); +// } - // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{first_goal}}; - publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); +// /** +// * @brief invalid_message Test mismatched joint and reference vector sizes +// */ +// TEST_P(TrajectoryControllerTestParameterized, invalid_message) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +// rclcpp::Parameter allow_integration_parameters( +// "allow_integration_in_goal_trajectories", false); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController( +// true, {partial_joints_parameters, allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // Incompatible joint names +// traj_msg = good_traj_msg; +// traj_msg.joint_names = {"bad_name"}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few efforts +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].effort = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Non-strictly increasing waypoint times +// traj_msg = good_traj_msg; +// traj_msg.points.push_back(traj_msg.points.front()); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } +// /// With allow_integration_in_goal_trajectories parameter trajectory missing position or +// /// velocities are accepted +// TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +// { +// rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// good_traj_msg.points[0].accelerations.resize(1); +// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position and velocity data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // All empty +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// traj_msg.points[0].accelerations.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } - executor.cancel(); -} +// /** +// * @brief test_trajectory_replace Test replacing an existing trajectory +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}}}; +// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; +// std::vector> points_partial_new{{1.5}}; +// std::vector> points_partial_new_velocities{{0.15}}; + +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_actual.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// expected_desired.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// // Check that we reached end of points_old trajectory +// // Denis: delta was 0.1 with 0.2 works for me +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + +// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); +// points_partial_new_velocities[0][0] = +// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); +// publish( +// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + +// // Replaced trajectory is a mix of previous and current goal +// expected_desired.positions[0] = points_partial_new[0][0]; +// expected_desired.positions[1] = points_old[0][1]; +// expected_desired.positions[2] = points_old[0][2]; +// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; +// expected_desired.velocities[1] = 0.0; +// expected_desired.velocities[2] = 0.0; +// expected_actual = expected_desired; +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } -TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); +// /** +// * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); + +// // TODO(anyone): add expectations for velocities and accelerations +// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; +// std::vector> points_new{{{-1., -2., -3.}}}; + +// RCLCPP_INFO( +// traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0] trajectory +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); +// // New trajectory will end before current time +// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; +// expected_desired = expected_actual; +// std::cout << "Sending old trajectory" << std::endl; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } - // goal setup - std::vector first_goal = {3.3, 4.4, 5.5}; - std::vector second_goal = {6.6, 8.8, 11.0}; - double state_from_command_offset = 0.3; +// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; +// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// RCLCPP_INFO( +// traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); +// // New trajectory first point is in the past, second is in the future +// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; +// expected_desired = expected_actual; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } - // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); +// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +// { +// SetUpTrajectoryController(); +// auto traj_node = traj_controller_->get_node(); +// RCLCPP_WARN( +// traj_node->get_logger(), +// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); +// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ +// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 +// return; + +// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(traj_node->get_node_base_interface()); +// subscribeToState(); +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// traj_node->set_parameter(partial_joints_parameters); +// traj_controller_->get_node()->configure(); +// traj_controller_->get_node()->activate(); + +// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; +// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; +// std::vector> partial_traj{ +// {{-1., -2.}, +// { +// -2., +// -4, +// }}}; +// std::vector> partial_traj_velocities{ +// {{-0.1, -0.2}, +// { +// -0.2, +// -0.4, +// }}}; +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; +// // Send full trajectory +// publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); +// // Sleep until first waypoint of full trajectory + +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// // Send partial trajectory starting after full trajecotry is complete +// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); +// publish( +// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); +// // Wait until the end start and end of partial traj + +// expected_actual.positions = { +// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; +// expected_desired = expected_actual; + +// waitAndCompareState( +// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); +// } - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } +// TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - executor.cancel(); -} +// // goal setup +// std::vector first_goal = {3.3, 4.4, 5.5}; +// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; +// std::vector second_goal = {6.6, 8.8, 11.0}; +// std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; +// double state_from_command_offset = 0.3; -// 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 -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); +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // JTC is executing trajectory in open-loop therefore: +// // - internal state does not have to be updated (in this test-case it shouldn't) +// // - internal command is updated +// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there should be a "jump" in opposite direction from command +// // (towards the state value) +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there should be a "jump" in the goal direction from command +// // (towards the state value) +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// } - // 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(true, {is_open_loop_parameters}, &executor, true); +// executor.cancel(); +// } - auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - for (size_t i = 0; i < 3; ++i) - { - 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() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - } - } +// // goal setup +// std::vector first_goal = {3.3, 4.4, 5.5}; +// std::vector second_goal = {6.6, 8.8, 11.0}; +// double state_from_command_offset = 0.3; - executor.cancel(); -} +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // JTC is executing trajectory in open-loop therefore: +// // - internal state does not have to be updated (in this test-case it shouldn't) +// // - internal command is updated +// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there **should not** be a "jump" in opposite direction from +// // command (towards the state value) +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be backward commands +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there **should not** be a "jump" in the goal direction from +// // command (towards the state value) +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be a jump toward commands +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// } -// Testing that values are read from state 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); +// executor.cancel(); +// } - // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = 3.1 + i; - joint_vel_[i] = 0.25 + i; - joint_acc_[i] = 0.02 + i / 10.0; - } - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); +// // 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 +// 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(true, {is_open_loop_parameters}, &executor, true); + +// 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_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() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } +// } - auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +// executor.cancel(); +// } - 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() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - } - } +// // Testing that values are read from state 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 +// for (size_t i = 0; i < 3; ++i) +// { +// joint_pos_[i] = 3.1 + i; +// joint_vel_[i] = 0.25 + i; +// joint_acc_[i] = 0.02 + i / 10.0; +// } +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// 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() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } +// } - executor.cancel(); -} +// executor.cancel(); +// } -// TODO(andyz): disabled because they started failing at the transition to Humble -/* // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, @@ -1271,30 +1290,31 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// only velocity controller -INSTANTIATE_TEST_SUITE_P( - OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"velocity"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"velocity"}), - std::vector({"position", "velocity", "acceleration"})))); - -// only effort controller -INSTANTIATE_TEST_SUITE_P( - OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"effort"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"effort"}), - std::vector({"position", "velocity", "acceleration"})))); -*/ +// // only velocity controller +// INSTANTIATE_TEST_SUITE_P( +// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"velocity"}), +// std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"velocity"}), +// std::vector({"position", "velocity", "acceleration"})))); + +// // only effort controller +// INSTANTIATE_TEST_SUITE_P( +// OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"effort"}), std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"effort"}), +// std::vector({"position", "velocity", "acceleration"})))); TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { - auto set_parameter_and_check_result = [&]() { + auto set_parameter_and_check_result = [&]() + { EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->get_node()->configure(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5c55d87972..f12bd8b8bc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -265,7 +265,9 @@ class TrajectoryControllerTest : public ::testing::Test // I do not understand why spin_some provides only one message qos.keep_last(1); state_subscriber_ = traj_lifecycle_node->create_subscription( - controller_name_ + "/state", qos, [&](std::shared_ptr msg) { + controller_name_ + "/state", qos, + [&](std::shared_ptr msg) + { std::lock_guard guard(state_mutex_); state_msg_ = msg; }); From e718ae9b1048593d0c961f6a4f92f3813f616d10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 15 Aug 2022 10:13:58 +0200 Subject: [PATCH 132/524] Fix formatting because pre-commit was not running on CI for some time. (#409) --- .pre-commit-config.yaml | 2 +- .../double_editor.py | 14 ++- .../joint_limits_urdf.py | 69 ++++++----- .../joint_trajectory_controller.py | 116 +++++++++--------- .../rqt_joint_trajectory_controller.py | 18 ++- .../update_combo.py | 7 +- .../rqt_joint_trajectory_controller/utils.py | 79 ++++++------ rqt_joint_trajectory_controller/setup.py | 1 - .../config/tricycle_drive_controller.yaml | 4 +- tricycle_controller/doc/userdoc.rst | 6 +- .../tricycle_controller/steering_limiter.hpp | 8 +- .../tricycle_controller/traction_limiter.hpp | 6 +- .../tricycle_controller.hpp | 7 +- tricycle_controller/src/steering_limiter.cpp | 4 +- tricycle_controller/src/traction_limiter.cpp | 6 +- .../src/tricycle_controller.cpp | 39 +++--- .../test/config/test_tricycle_controller.yaml | 2 +- .../test/test_load_tricycle_controller.cpp | 4 +- 18 files changed, 203 insertions(+), 189 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa8d75100e..58fe46dd90 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -139,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '--uri-ignore-words-list=ist'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py index 3fdde9daf9..9a96639436 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -43,6 +43,7 @@ class DoubleEditor(QWidget): # DoubleEditor? """ Widget that allows to edit the value of a floating-point value. + Optionally subject to lower and upper bounds. """ @@ -59,15 +60,17 @@ def __init__(self, min_val, max_val): self._max_val = max_val # Load editor UI - ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), - 'resource', 'double_editor.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_joint_trajectory_controller"), + "resource", + "double_editor.ui", + ) loadUi(ui_file, self) # Setup widget ranges and slider scale factor self.slider.setRange(0, 100) self.slider.setSingleStep(1) - self._scale = (max_val - min_val) / \ - (self.slider.maximum() - self.slider.minimum()) + self._scale = (max_val - min_val) / (self.slider.maximum() - self.slider.minimum()) self.spin_box.setRange(min_val, max_val) self.spin_box.setSingleStep(self._scale) @@ -82,8 +85,7 @@ def _slider_to_val(self, sval): return self._min_val + self._scale * (sval - self.slider.minimum()) def _val_to_slider(self, val): - return round(self.slider.minimum() + (val - self._min_val) / - self._scale) + return round(self.slider.minimum() + (val - self._min_val) / self._scale) def _on_slider_changed(self): val = self._slider_to_val(self.slider.value()) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 6f339ba387..d7517b1847 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -19,7 +33,7 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True): +def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -39,53 +53,50 @@ def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=Tr dependent_joints = {} if description != "": - robot = xml.dom.minidom.parseString(description)\ - .getElementsByTagName('robot')[0] + robot = xml.dom.minidom.parseString(description).getElementsByTagName("robot")[0] # Find all non-fixed joints for child in robot.childNodes: if child.nodeType is child.TEXT_NODE: continue - if child.localName == 'joint': - jtype = child.getAttribute('type') - if jtype == 'fixed': + if child.localName == "joint": + jtype = child.getAttribute("type") + if jtype == "fixed": continue - name = child.getAttribute('name') + name = child.getAttribute("name") try: - limit = child.getElementsByTagName('limit')[0] + limit = child.getElementsByTagName("limit")[0] except: continue - if jtype == 'continuous': + if jtype == "continuous": minval = -pi maxval = pi else: try: - minval = float(limit.getAttribute('lower')) - maxval = float(limit.getAttribute('upper')) + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) except: continue try: - maxvel = float(limit.getAttribute('velocity')) + maxvel = float(limit.getAttribute("velocity")) except: continue - safety_tags = child.getElementsByTagName('safety_controller') + safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: tag = safety_tags[0] - if tag.hasAttribute('soft_lower_limit'): - minval = max(minval, - float(tag.getAttribute('soft_lower_limit'))) - if tag.hasAttribute('soft_upper_limit'): - maxval = min(maxval, - float(tag.getAttribute('soft_upper_limit'))) - - mimic_tags = child.getElementsByTagName('mimic') + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") if use_mimic and len(mimic_tags) == 1: tag = mimic_tags[0] - entry = {'parent': tag.getAttribute('joint')} - if tag.hasAttribute('multiplier'): - entry['factor'] = float(tag.getAttribute('multiplier')) - if tag.hasAttribute('offset'): - entry['offset'] = float(tag.getAttribute('offset')) + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) dependent_joints[name] = entry continue @@ -93,8 +104,8 @@ def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=Tr if name in dependent_joints: continue - joint = {'min_position': minval, 'max_position': maxval} - joint["has_position_limits"] = jtype != 'continuous' - joint['max_velocity'] = maxvel + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 7cb9ea5346..ab55d6eb78 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -99,7 +99,7 @@ class JointTrajectoryController(Plugin): def __init__(self, context): super().__init__(context) - self.setObjectName('JointTrajectoryController') + self.setObjectName("JointTrajectoryController") self._node = rclpy.node.Node("rqt_joint_trajectory_controller") self._executor = None self._executor_thread = None @@ -107,17 +107,19 @@ def __init__(self, context): # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() - ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), - 'resource', - 'joint_trajectory_controller.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_joint_trajectory_controller"), + "resource", + "joint_trajectory_controller.ui", + ) loadUi(ui_file, self._widget) - self._widget.setObjectName('JointTrajectoryControllerUi') + self._widget.setObjectName("JointTrajectoryControllerUi") ns = self._node.get_namespace()[1:-1] - self._widget.controller_group.setTitle('ns: ' + ns) + self._widget.controller_group.setTitle("ns: " + ns) # Setup speed scaler speed_scaling = DoubleEditor(1.0, 100.0) - speed_scaling.spin_box.setSuffix('%') + speed_scaling.spin_box.setSuffix("%") speed_scaling.spin_box.setValue(50.0) speed_scaling.spin_box.setDecimals(0) speed_scaling.setEnabled(False) @@ -132,8 +134,9 @@ def __init__(self, context): # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) + self._widget.setWindowTitle( + self._widget.windowTitle() + (" (%d)" % context.serial_number()) + ) # Add widget to the user interface context.add_widget(self._widget) @@ -151,22 +154,19 @@ def __init__(self, context): # Timer for updating the joint widgets from the controller state self._update_act_pos_timer = QTimer(self) - self._update_act_pos_timer.setInterval(int(1000.0 / - self._widget_update_freq)) + self._update_act_pos_timer.setInterval(int(1000.0 / self._widget_update_freq)) self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) - self._update_cm_list_timer.setInterval(int(1000.0 / - self._ctrlrs_update_freq)) + self._update_cm_list_timer.setInterval(int(1000.0 / self._ctrlrs_update_freq)) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_jtc_list_timer = QTimer(self) - self._update_jtc_list_timer.setInterval(int(1000.0 / - self._ctrlrs_update_freq)) + self._update_jtc_list_timer.setInterval(int(1000.0 / self._ctrlrs_update_freq)) self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() @@ -176,7 +176,7 @@ def __init__(self, context): w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) - self._cmd_pub = None # Controller command publisher + self._cmd_pub = None # Controller command publisher self._state_sub = None # Controller state subscriber self._list_controllers = None @@ -191,13 +191,13 @@ def shutdown_plugin(self): self._unregister_executor() def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('cm_ns', self._cm_ns) - instance_settings.set_value('jtc_name', self._jtc_name) + instance_settings.set_value("cm_ns", self._cm_ns) + instance_settings.set_value("jtc_name", self._jtc_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() - cm_ns = instance_settings.value('cm_ns') + cm_ns = instance_settings.value("cm_ns") cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: @@ -205,7 +205,7 @@ def restore_settings(self, plugin_settings, instance_settings): cm_combo.setCurrentIndex(idx) # Restore last session's controller, if running self._update_jtc_list() - jtc_name = instance_settings.value('jtc_name') + jtc_name = instance_settings.value("jtc_name") jtc_combo = self._widget.jtc_combo jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] try: @@ -217,10 +217,10 @@ def restore_settings(self, plugin_settings, instance_settings): pass # def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure - # This will enable a setting button (gear icon) in each dock widget - # title bar - # Usually used to open a modal configuration dialog + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) @@ -237,10 +237,11 @@ def _update_jtc_list(self): if running_jtc and not self._robot_joint_limits: self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation valid_jtc = [] - if (self._robot_joint_limits): + if self._robot_joint_limits: for jtc_info in running_jtc: - has_limits = all(name in self._robot_joint_limits - for name in _jtc_joint_names(jtc_info)) + has_limits = all( + name in self._robot_joint_limits for name in _jtc_joint_names(jtc_info) + ) if has_limits: valid_jtc.append(jtc_info) valid_jtc_names = [data.name for data in valid_jtc] @@ -252,9 +253,9 @@ def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() def _on_joint_state_change(self, actual_pos): - assert(len(actual_pos) == len(self._joint_pos)) + assert len(actual_pos) == len(self._joint_pos) for name in actual_pos.keys(): - self._joint_pos[name]['position'] = actual_pos[name] + self._joint_pos[name]["position"] = actual_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns @@ -299,8 +300,9 @@ def _on_jtc_enabled(self, val): def _load_jtc(self): # Initialize joint data corresponding to selected controller running_jtc = self._running_jtc_info() - self._joint_names = next(_jtc_joint_names(x) for x in running_jtc - if x.name == self._jtc_name) + self._joint_names = next( + _jtc_joint_names(x) for x in running_jtc if x.name == self._jtc_name + ) for name in self._joint_names: self._joint_pos[name] = {} @@ -309,35 +311,33 @@ def _load_jtc(self): layout = self._widget.joint_group.layout() for name in self._joint_names: limits = self._robot_joint_limits[name] - joint_widget = DoubleEditor(limits['min_position'], - limits['max_position']) + joint_widget = DoubleEditor(limits["min_position"], limits["max_position"]) layout.addRow(name, joint_widget) # NOTE: Using partial instead of a lambda because lambdas # "will not evaluate/look up the argument values before it is # effectively called, breaking situations like using a loop # variable inside it" from functools import partial + par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) except: # TODO: Can we do better than swallow the exception? from sys import exc_info - print('Unexpected error:', exc_info()[0]) + + print("Unexpected error:", exc_info()[0]) # Enter monitor mode (sending commands disabled) self._on_jtc_enabled(False) # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) - state_topic = jtc_ns + '/state' - cmd_topic = jtc_ns + '/joint_trajectory' - self._state_sub = self._node.create_subscription(JointTrajectoryControllerState, - state_topic, - self._state_cb, - 1) - self._cmd_pub = self._node.create_publisher(JointTrajectory, - cmd_topic, - 1) + state_topic = jtc_ns + "/state" + cmd_topic = jtc_ns + "/joint_trajectory" + self._state_sub = self._node.create_subscription( + JointTrajectoryControllerState, state_topic, self._state_cb, 1 + ) + self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1) self.jointStateChanged.connect(self._on_joint_state_change) @@ -381,10 +381,10 @@ def _running_jtc_info(self): from .utils import filter_by_type, filter_by_state controller_list = self._list_controllers() - jtc_list = filter_by_type(controller_list, - 'JointTrajectoryController', - match_substring=True) - running_jtc_list = filter_by_state(jtc_list, 'active') + jtc_list = filter_by_type( + controller_list, "JointTrajectoryController", match_substring=True + ) + running_jtc_list = filter_by_state(jtc_list, "active") return running_jtc_list def _unregister_cmd_pub(self): @@ -412,7 +412,7 @@ def _state_cb(self, msg): self.jointStateChanged.emit(actual_pos) def _update_single_cmd_cb(self, val, name): - self._joint_pos[name]['command'] = val + self._joint_pos[name]["command"] = val def _update_cmd_cb(self): dur = [] @@ -420,13 +420,13 @@ def _update_cmd_cb(self): traj.joint_names = self._joint_names point = JointTrajectoryPoint() for name in traj.joint_names: - pos = self._joint_pos[name]['position'] + pos = self._joint_pos[name]["position"] cmd = pos try: - cmd = self._joint_pos[name]['command'] + cmd = self._joint_pos[name]["command"] except (KeyError): pass - max_vel = self._robot_joint_limits[name]['max_velocity'] + max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) point.positions.append(cmd) duration = rclpy.duration.Duration(seconds=(max(dur) / self._speed_scale)) @@ -440,7 +440,7 @@ def _update_joint_widgets(self): for id in range(len(joint_widgets)): joint_name = self._joint_names[id] try: - joint_pos = self._joint_pos[joint_name]['position'] + joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) except (KeyError): pass # Can happen when first connected to controller @@ -449,8 +449,7 @@ def _joint_widgets(self): # TODO: Cache instead of compute every time? widgets = [] layout = self._widget.joint_group.layout() for row_id in range(layout.rowCount()): - widgets.append(layout.itemAt(row_id, - QFormLayout.FieldRole).widget()) + widgets.append(layout.itemAt(row_id, QFormLayout.FieldRole).widget()) return widgets @@ -470,6 +469,7 @@ def _jtc_joint_names(jtc_info): def _resolve_controller_ns(cm_ns, controller_name): """ Resolve a controller's namespace from that of the controller manager. + Controllers are assumed to live one level above the controller manager, e.g. @@ -491,9 +491,9 @@ def _resolve_controller_ns(cm_ns, controller_name): @return Controller namespace @rtype str """ - assert(controller_name) - ns = cm_ns.rsplit('/', 1)[0] - if ns != '/': - ns += '/' + assert controller_name + ns = cm_ns.rsplit("/", 1)[0] + if ns != "/": + ns += "/" ns += controller_name return ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py index d0eb860fc5..847a9e04fb 100755 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py @@ -1,8 +1,22 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. import sys from rqt_gui.main import Main main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller')) +sys.exit(main.main(sys.argv, standalone="rqt_joint_trajectory_controller")) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 64e9565cd6..4702d7fbc5 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -48,9 +48,10 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """ - @type a [] - @type b [] + """Check is arrays are permutation of each other. + + @type a [] first array with values to compare with the second one. + @type b [] second array with values to compare with the first one. @return True if C{a} is a permutation of C{b}, false otherwise @rtype bool """ diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index a84eb2a5f8..577cc1ab07 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -23,32 +23,31 @@ from controller_manager_msgs.srv import ListControllers # Names of controller manager services, and their respective types -_LIST_CONTROLLERS_STR = 'list_controllers' -_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/srv/ListControllers' -_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' -_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/srv/ListControllerTypes' -_LOAD_CONTROLLER_STR = 'load_controller' -_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/LoadController' -_UNLOAD_CONTROLLER_STR = 'unload_controller' -_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/UnloadController' -_SWITCH_CONTROLLER_STR = 'switch_controller' -_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/srv/SwitchController' -_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' -_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/srv/' - 'ReloadControllerLibraries') +_LIST_CONTROLLERS_STR = "list_controllers" +_LIST_CONTROLLERS_TYPE = "controller_manager_msgs/srv/ListControllers" +_LIST_CONTROLLER_TYPES_STR = "list_controller_types" +_LIST_CONTROLLER_TYPES_TYPE = "controller_manager_msgs/srv/ListControllerTypes" +_LOAD_CONTROLLER_STR = "load_controller" +_LOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/LoadController" +_UNLOAD_CONTROLLER_STR = "unload_controller" +_UNLOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/UnloadController" +_SWITCH_CONTROLLER_STR = "switch_controller" +_SWITCH_CONTROLLER_TYPE = "controller_manager_msgs/srv/SwitchController" +_RELOAD_CONTROLLER_LIBS_STR = "reload_controller_libraries" +_RELOAD_CONTROLLER_LIBS_TYPE = "controller_manager_msgs/srv/" "ReloadControllerLibraries" # Map from service names to their respective type cm_services = { - _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, - _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, - _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, - _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, - _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, - _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE, } -def get_controller_managers(namespace='/', initial_guess=None): +def get_controller_managers(namespace="/", initial_guess=None): """ Get list of active controller manager namespaces. @@ -78,8 +77,7 @@ def get_controller_managers(namespace='/', initial_guess=None): # 1. Remove entries not found in current list # 2. Add new untracked controller managers ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] - ns_list += [ns for ns in ns_list_curr - if ns not in ns_list and is_controller_manager(node, ns)] + ns_list += [ns for ns in ns_list_curr if ns not in ns_list and is_controller_manager(node, ns)] return sorted(ns_list) @@ -97,8 +95,8 @@ def is_controller_manager(node, namespace): @rtype: bool """ cm_ns = namespace - if not cm_ns or cm_ns[-1] != '/': - cm_ns += '/' + if not cm_ns or cm_ns[-1] != "/": + cm_ns += "/" for srv_name in cm_services.keys(): if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): return False @@ -126,14 +124,14 @@ def _sloppy_get_controller_managers(node, namespace): ns_list = [] for srv_info in srv_list: - match_str = '/' + _LIST_CONTROLLERS_STR + match_str = "/" + _LIST_CONTROLLERS_STR # First element of srv_name is the service name if match_str in srv_info[0]: ns = srv_info[0].split(match_str)[0] - if ns == '': + if ns == "": # controller manager services live in root namespace # unlikely, but still possible - ns = '/' + ns = "/" ns_list.append(ns) return ns_list @@ -164,6 +162,7 @@ def _srv_exists(node, srv_name, srv_type): # ############################################################################### + class ControllerManagerLister: """ Convenience functor for querying the list of active controller managers. @@ -177,7 +176,7 @@ class ControllerManagerLister: >>> print(list_cm()) """ - def __init__(self, namespace='/'): + def __init__(self, namespace="/"): self._ns = namespace self._cm_list = [] @@ -201,14 +200,14 @@ class ControllerLister: >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') """ - def __init__(self, namespace='/controller_manager'): + def __init__(self, namespace="/controller_manager"): """ @param namespace Namespace of controller manager to monitor. @type namespace str """ self._node = rclpy.node.Node("controller_lister") - self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv_name = namespace + "/" + _LIST_CONTROLLERS_STR self._srv_client = self._create_client() """ @@ -224,6 +223,7 @@ def __call__(self): def _create_client(self): return self._node.create_client(ListControllers, self._srv_name) + ############################################################################### # # Convenience methods for filtering controller state information @@ -244,7 +244,7 @@ def filter_by_name(ctrl_list, ctrl_name, match_substring=False): @return: Controllers matching the specified name @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + return _filter_by_attr(ctrl_list, "name", ctrl_name, match_substring) def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @@ -260,7 +260,7 @@ def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @return: Controllers matching the specified type @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + return _filter_by_attr(ctrl_list, "type", ctrl_type, match_substring) def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @@ -276,12 +276,10 @@ def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @return: Controllers matching the specified state @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + return _filter_by_attr(ctrl_list, "state", ctrl_state, match_substring) -def filter_by_hardware_interface(ctrl_list, - hardware_interface, - match_substring=False): +def filter_by_hardware_interface(ctrl_list, hardware_interface, match_substring=False): """ Filter controller state list by controller hardware interface. @@ -308,10 +306,7 @@ def filter_by_hardware_interface(ctrl_list, return list_out -def filter_by_resources(ctrl_list, - resources, - hardware_interface=None, - match_any=False): +def filter_by_resources(ctrl_list, resources, hardware_interface=None, match_any=False): """ Filter controller state list by claimed resources. @@ -342,8 +337,7 @@ def filter_by_resources(ctrl_list, list_out = [] for ctrl in ctrl_list: for resource_set in ctrl.claimed_resources: - if (hardware_interface is None or - hardware_interface == resource_set.hardware_interface): + if hardware_interface is None or hardware_interface == resource_set.hardware_interface: for res in resources: add_ctrl = not match_any # Initial flag value if res in resource_set.resources: @@ -372,6 +366,7 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): list_out.append(val) return list_out + ############################################################################### # # Convenience methods for finding potential controller configurations diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 5f24382cb4..f5dc707899 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -14,7 +14,6 @@ ("share/" + package_name, ["package.xml"]), ("share/" + package_name + "/resource", glob("resource/*.*")), ("share/" + package_name, ["plugin.xml"]), - ], install_requires=["setuptools"], zip_safe=True, diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index 4eabf615ef..c1077b13c3 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -1,4 +1,4 @@ -tricycle_controller: +tricycle_controller: ros__parameters: # Model traction_joint_name: traction_joint # Name of traction joint in URDF @@ -16,7 +16,7 @@ tricycle_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - + # Rate Limiting traction: # All values should be positive # min_velocity: 0.0 diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2271b8c9cf..a248bbec96 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,8 +10,8 @@ commands for the tricycle drive base. Odometry is computed from hardware feedbac Velocity commands ----------------- -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. @@ -21,4 +21,4 @@ Other features Realtime-safe implementation. Odometry publishing Velocity, acceleration and jerk limits - Automatic stop after command timeout \ No newline at end of file + Automatic stop after command timeout diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp index 51ecac4cbd..a16f0de0f0 100644 --- a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class SteeringLimiter { public: @@ -37,9 +36,8 @@ class SteeringLimiter * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] */ SteeringLimiter( - double min_position = NAN, double max_position = NAN, - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN); + double min_position = NAN, double max_position = NAN, double min_velocity = NAN, + double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); /** * \brief Limit the position, velocity and acceleration @@ -78,7 +76,6 @@ class SteeringLimiter double limit_acceleration(double & p, double p0, double p1, double dt); private: - // Position limits: double min_position_; double max_position_; @@ -90,7 +87,6 @@ class SteeringLimiter // Acceleration limits: double min_acceleration_; double max_acceleration_; - }; } // namespace tricycle_controller diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index 4c0f297ee8..ea0bb16025 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class TractionLimiter { public: @@ -39,9 +38,8 @@ class TractionLimiter * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ TractionLimiter( - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, - double min_deceleration = NAN, double max_deceleration = NAN, + double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, + double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); /** diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index c5ba1c62e3..2178008725 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include "ackermann_msgs/msg/ackermann_drive.hpp" @@ -32,16 +33,16 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "std_srvs/srv/empty.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" -#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" namespace tricycle_controller @@ -125,7 +126,7 @@ class TricycleController : public controller_interface::ControllerInterface { bool open_loop = false; bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in seperate node + bool odom_only_twist = false; // for doing the pose integration in separate node std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; std::array pose_covariance_diagonal; diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index 6912144bd0..c1649f12dc 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -87,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -106,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index c2a0d88ab4..7865d4be71 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -91,7 +91,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +113,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +132,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 7cec107a91..c930086701 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -151,7 +151,7 @@ controller_interface::return_type TricycleController::update( double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - + if (odom_params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); @@ -160,7 +160,7 @@ controller_interface::return_type TricycleController::update( { if (std::isnan(Ws_read) || std::isnan(alpha_read)) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); return controller_interface::return_type::ERROR; } odometry_.update(Ws_read, alpha_read, period); @@ -208,7 +208,7 @@ controller_interface::return_type TricycleController::update( // Compute wheel velocity and angle auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); - // Reduce wheel speed until the target angle has been reached + // Reduce wheel speed until the target angle has been reached double alpha_delta = abs(alpha_write - alpha_read); double scale; if (alpha_delta < M_PI / 6) @@ -221,7 +221,7 @@ controller_interface::return_type TricycleController::update( } else { - // TODO: find the best function, e.g convex power functions + // TODO(anyone): find the best function, e.g convex power functions scale = cos(alpha_delta); } Ws_write *= scale; @@ -238,8 +238,9 @@ controller_interface::return_type TricycleController::update( previous_commands_.pop(); AckermannDrive ackermann_command; - ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel + // speed (rad/s) + ackermann_command.speed = Ws_write; ackermann_command.steering_angle = alpha_write; previous_commands_.emplace(ackermann_command); @@ -247,8 +248,9 @@ controller_interface::return_type TricycleController::update( if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - realtime_ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel + // speed (rad/s) + realtime_ackermann_command.speed = Ws_write; realtime_ackermann_command.steering_angle = alpha_write; realtime_ackermann_command_publisher_->unlockAndPublish(); } @@ -298,7 +300,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + cmd_vel_timeout_ = + std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); @@ -367,8 +370,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -390,8 +392,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -558,8 +559,7 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&traction_joint_name](const auto & interface) - { + [&traction_joint_name](const auto & interface) { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -574,8 +574,7 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&traction_joint_name](const auto & interface) - { + [&traction_joint_name](const auto & interface) { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -600,8 +599,7 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steering_joint_name](const auto & interface) - { + [&steering_joint_name](const auto & interface) { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); @@ -616,8 +614,7 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&steering_joint_name](const auto & interface) - { + [&steering_joint_name](const auto & interface) { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index d81d722219..efecfe968f 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -17,4 +17,4 @@ test_tricycle_controller: max_deceleration: 8.0 steering: max_position: 1.57 # pi/2 - max_velocity: 1.0 \ No newline at end of file + max_velocity: 1.0 diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 088cddbabe..d04b83ae27 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -36,8 +36,8 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NO_THROW( + cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); rclcpp::shutdown(); } From cbb9feb9af95a99dd7e8366c0af0a3c067d37523 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 16 Aug 2022 07:29:55 +0100 Subject: [PATCH 133/524] Update reviewers from ros2_control repo --- .github/reviewer-lottery.yml | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index a5a67c6b56..50c707e12b 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -14,19 +14,15 @@ groups: reviewers: 5 usernames: - rosterloh - - kellyprankin - progtologist - arne48 - DasRoteSkelett - - a10263790 - Serafadam - harderthan - jaron-l - malapatiravi - - erickisos - - Briancbn - - TomoyaFujita2016 - homalozoa + - erickisos - anfemosa - jackcenter - VX792 @@ -34,9 +30,7 @@ groups: - livanov93 - aprotyas - peterdavidfagan - - UsamaHamayun1 - duringhof - bijoua29 - - kasiceo - lm2292 - mcbed From 0da387d7ee533bd002ddbce5255450b4709168de Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 17 Aug 2022 11:42:00 -0600 Subject: [PATCH 134/524] Fix formatting CI job (#418) Signed-off-by: Tyler Weaver --- .../src/diff_drive_controller.cpp | 15 ++++++++++----- .../gripper_action_controller_impl.hpp | 15 ++++++--------- .../src/joint_state_broadcaster.cpp | 3 ++- .../src/tricycle_controller.cpp | 18 ++++++++++++------ 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 48ebdc9fb7..bfb5baf3c3 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -423,7 +423,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -446,7 +447,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -595,7 +597,8 @@ controller_interface::CallbackReturn DiffDriveController::on_shutdown( void DiffDriveController::halt() { - const auto halt_wheels = [](auto & wheel_handles) { + const auto halt_wheels = [](auto & wheel_handles) + { for (const auto & wheel_handle : wheel_handles) { wheel_handle.velocity.get().set_value(0.0); @@ -625,7 +628,8 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto interface_name = feedback_type(); const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&wheel_name, &interface_name](const auto & interface) { + [&wheel_name, &interface_name](const auto & interface) + { return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == interface_name; }); @@ -638,7 +642,8 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&wheel_name](const auto & interface) { + [&wheel_name](const auto & interface) + { return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == HW_IF_VELOCITY; }); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 0b5f791e78..ee07a9c0a0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -238,9 +238,8 @@ controller_interface::CallbackReturn GripperActionController: { auto position_command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [](const hardware_interface::LoanedCommandInterface & command_interface) { - return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; - }); + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); if (position_command_interface_it == command_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); @@ -256,9 +255,8 @@ controller_interface::CallbackReturn GripperActionController: } const auto position_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; - }); + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); if (position_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); @@ -274,9 +272,8 @@ controller_interface::CallbackReturn GripperActionController: } const auto velocity_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; - }); + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); if (velocity_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 65d9ec87c2..18d5f25e60 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -123,7 +123,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); } - auto get_map_interface_parameter = [&](const std::string & interface) { + auto get_map_interface_parameter = [&](const std::string & interface) + { std::string interface_to_map = get_node() ->get_parameter(std::string("map_interface_to_joint_state.") + interface) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index c930086701..114731242c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -370,7 +370,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -392,7 +393,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -559,7 +561,8 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&traction_joint_name](const auto & interface) { + [&traction_joint_name](const auto & interface) + { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -574,7 +577,8 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&traction_joint_name](const auto & interface) { + [&traction_joint_name](const auto & interface) + { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -599,7 +603,8 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steering_joint_name](const auto & interface) { + [&steering_joint_name](const auto & interface) + { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); @@ -614,7 +619,8 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&steering_joint_name](const auto & interface) { + [&steering_joint_name](const auto & interface) + { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); From abd83df619ef94d33cccfcfdf0a23a7f2a44c82a Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Wed, 17 Aug 2022 15:42:57 -0500 Subject: [PATCH 135/524] build: :construction_worker: remove foxy rhel build (#420) - there is no foxy rhel docker image because it osrf doesn't publish rhel packages for foxy --- .github/workflows/foxy-rhel-binary-build.yml | 33 -------------------- 1 file changed, 33 deletions(-) delete mode 100644 .github/workflows/foxy-rhel-binary-build.yml diff --git a/.github/workflows/foxy-rhel-binary-build.yml b/.github/workflows/foxy-rhel-binary-build.yml deleted file mode 100644 index 8ce90c940f..0000000000 --- a/.github/workflows/foxy-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Foxy RHEL Binary Build -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - foxy_rhel_binary: - name: Foxy RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: foxy - container: jaronl/ros:foxy-alma - steps: - - uses: actions/checkout@v3 - 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 From 33ff4eadca892d5fcaab372d3ed0c7d8020303b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 18 Aug 2022 09:20:59 +0200 Subject: [PATCH 136/524] Add all packages into ros-lint-ci (#419) --- .github/workflows/ci-ros-lint.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 9902dbdaaf..d8f77a323a 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -19,6 +19,17 @@ jobs: linter: ${{ matrix.linter }} package-name: diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + gripper_controllers + position_controllers + ros2_controllers + ros2_controllers_test_nodes + tricycle_controller + velocity_controllers ament_lint_100: @@ -38,4 +49,14 @@ jobs: arguments: "--linelength=100 --filter=-whitespace/newline" package-name: diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + gripper_controllers + position_controllers ros2_controllers + ros2_controllers_test_nodes + tricycle_controller + velocity_controllers From 8c2547c8ed7e626ff5e18ecd1f079a20760b449c Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Thu, 18 Aug 2022 13:03:48 -0500 Subject: [PATCH 137/524] test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (#415) --- .../test/test_trajectory_controller.cpp | 88 ++++++++++--------- 1 file changed, 45 insertions(+), 43 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 576f6a30d7..0d717515cf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -356,55 +356,57 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } -// TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); -// updateController(); +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(true, {}, &executor); + subscribeToState(); + updateController(); -// // Spin to receive latest state -// executor.spin_some(); -// auto state = getState(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); -// size_t n_joints = joint_names_.size(); + size_t n_joints = joint_names_.size(); -// for (unsigned int i = 0; i < n_joints; ++i) -// { -// EXPECT_EQ(joint_names_[i], state->joint_names[i]); -// } + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_EQ(joint_names_[i], state->joint_names[i]); + } -// // No trajectory by default, no desired state or error -// EXPECT_TRUE(state->desired.positions.empty()); -// EXPECT_TRUE(state->desired.velocities.empty()); -// EXPECT_TRUE(state->desired.accelerations.empty()); + // No trajectory by default, no desired state or error + EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS); + EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); -// EXPECT_EQ(n_joints, state->actual.positions.size()); -// if ( -// std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == -// state_interface_types_.end()) -// { -// EXPECT_TRUE(state->actual.velocities.empty()); -// } -// else -// { -// EXPECT_EQ(n_joints, state->actual.velocities.size()); -// } -// if ( -// std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == -// state_interface_types_.end()) -// { -// EXPECT_TRUE(state->actual.accelerations.empty()); -// } -// else -// { -// EXPECT_EQ(n_joints, state->actual.accelerations.size()); -// } + EXPECT_EQ(n_joints, state->actual.positions.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.velocities.size()); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.accelerations.size()); + } -// EXPECT_TRUE(state->error.positions.empty()); -// EXPECT_TRUE(state->error.velocities.empty()); -// EXPECT_TRUE(state->error.accelerations.empty()); -// } + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); +} // void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) // { From 9e08ee3d05ec9212349ff5b687afcd7c2add8a02 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Thu, 18 Aug 2022 13:14:15 -0500 Subject: [PATCH 138/524] fix: :bug: make bare exceptions more narrow (#422) - fixes pre-commit pipeline --- .../rqt_joint_trajectory_controller/joint_limits_urdf.py | 6 +++--- .../joint_trajectory_controller.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index d7517b1847..7914a76b17 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -66,7 +66,7 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr name = child.getAttribute("name") try: limit = child.getElementsByTagName("limit")[0] - except: + except IndexError: continue if jtype == "continuous": minval = -pi @@ -75,11 +75,11 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr try: minval = float(limit.getAttribute("lower")) maxval = float(limit.getAttribute("upper")) - except: + except ValueError: continue try: maxvel = float(limit.getAttribute("velocity")) - except: + except ValueError: continue safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index ab55d6eb78..7e8ec4948e 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -321,7 +321,7 @@ def _load_jtc(self): par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) - except: + except Exception: # TODO: Can we do better than swallow the exception? from sys import exc_info @@ -350,7 +350,7 @@ def _unload_jtc(self): # Stop updating the joint positions try: self.jointStateChanged.disconnect(self._on_joint_state_change) - except: + except Exception: pass # Reset ROS interfaces From 93fa9fec93321960756429830b661ca5d0b3b013 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 21 Aug 2022 09:18:28 +0200 Subject: [PATCH 139/524] [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (#380) * [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. * Use command_joint_states correctly in parameters depending on joint states. * Fixup tests. --- joint_trajectory_controller/doc/userdoc.rst | 27 ++++++- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 32 ++++++-- .../test_load_joint_trajectory_controller.cpp | 3 +- .../test/test_trajectory_controller.cpp | 81 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 8 ++ 6 files changed, 143 insertions(+), 9 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index ec58a423f1..17c0c511c5 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -32,6 +32,26 @@ Other features Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +(the controller is not yet implemented as chainable controller) + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). +Legal combinations of state interfaces are: +- ``position`` +- ``position`` and ``velocity`` +- ``position``, ``velocity`` and ``acceleration`` +- ``effort`` + +Commands +^^^^^^^^^ + Using Joint Trajectory Controller(s) ------------------------------------ @@ -84,8 +104,11 @@ A yaml file for using it could be: Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joint (list(string)): - Joint names to control. +joints (list(string)): + Joint names to control and listen to. + +command_joints (list(string)): + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. command_interface (list(string)): Command interfaces provided by the hardware interface for all joints. 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 4c2f612112..79fe6bccf4 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 @@ -113,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5898a6fe95..64092e1ab5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters joint_names_ = auto_declare>("joints", joint_names_); + command_joint_names_ = + auto_declare>("command_joints", command_joint_names_); command_interface_types_ = auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = @@ -97,7 +99,7 @@ JointTrajectoryController::command_interface_configuration() const std::exit(EXIT_FAILURE); } conf.names.reserve(dof_ * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -493,6 +495,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( dof_ = joint_names_.size(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -500,9 +503,25 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (joint_names_.empty()) { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -592,12 +611,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Init PID gains from ROS parameter server for (size_t i = 0; i < pids_.size(); ++i) { - const std::string prefix = "gains." + joint_names_[i]; + const std::string prefix = "gains." + command_joint_names_[i]; const auto k_p = auto_declare(prefix + ".p", 0.0); const auto k_i = auto_declare(prefix + ".i", 0.0); const auto k_d = auto_declare(prefix + ".d", 0.0); const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); + ff_velocity_scale_[i] = + auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); // Initialize PID pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); } @@ -702,7 +722,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); + default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); // Read parameters customizing controller for special cases open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); @@ -738,7 +758,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); - state_publisher_->msg_.joint_names = joint_names_; + state_publisher_->msg_.joint_names = command_joint_names_; state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); @@ -797,7 +817,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index e0dfdc6353..0f94d0b16c 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include "gmock/gmock.h" + #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0d717515cf..cc5b175281 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -95,6 +95,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, check_interface_names) +{ + SetUpTrajectoryController(); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) +{ + SetUpTrajectoryController(); + + // set command_joints parameter + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); + traj_controller_->get_node()->set_parameter({command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + TEST_P(TrajectoryControllerTestParameterized, activate) { SetUpTrajectoryController(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f12bd8b8bc..be11d274ad 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -81,6 +81,11 @@ class TestableJointTrajectoryController void set_joint_names(const std::vector & joint_names) { joint_names_ = joint_names; } + void set_command_joint_names(const std::vector & command_joint_names) + { + command_joint_names_ = command_joint_names; + } + void set_command_interfaces(const std::vector & command_interfaces) { command_interface_types_ = command_interfaces; @@ -115,6 +120,8 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ = "test_joint_trajectory_controller"; joint_names_ = {"joint1", "joint2", "joint3"}; + command_joint_names_ = { + "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"}; joint_pos_.resize(joint_names_.size(), 0.0); joint_state_pos_.resize(joint_names_.size(), 0.0); joint_vel_.resize(joint_names_.size(), 0.0); @@ -396,6 +403,7 @@ class TrajectoryControllerTest : public ::testing::Test std::string controller_name_; std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; From 5e6ef29bc4c42461b435e8a168cc4081f7db354e Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 27 Aug 2022 10:53:44 -0600 Subject: [PATCH 140/524] Generate params for ForceTorqueSensorBroadcaster (#395) Signed-off-by: Tyler Weaver --- .../CMakeLists.txt | 8 ++++ .../force_torque_sensor_broadcaster.hpp | 6 +-- force_torque_sensor_broadcaster/package.xml | 2 + .../src/force_torque_sensor_broadcaster.cpp | 41 +++++++---------- ..._torque_sensor_broadcaster_parameters.yaml | 44 +++++++++++++++++++ ros2_controllers.rolling.repos | 4 ++ 6 files changed, 77 insertions(+), 28 deletions(-) create mode 100644 force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index a710586700..cdb1fe2b35 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -26,6 +26,11 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) +generate_parameter_library(force_torque_sensor_broadcaster_parameters + src/force_torque_sensor_broadcaster_parameters.yaml +) + add_library( ${PROJECT_NAME} SHARED @@ -38,6 +43,9 @@ ament_target_dependencies( ${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +target_link_libraries(force_torque_sensor_broadcaster + force_torque_sensor_broadcaster_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 023c59cae0..6166adfd9a 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" +#include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -64,9 +65,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::string sensor_name_; - std::array interface_names_; - std::string frame_id_; + std::shared_ptr param_listener_; + Params params_; std::unique_ptr force_torque_sensor_; diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 868d83fdea..de4a0e0513 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,6 +11,8 @@ ament_cmake + generate_parameter_library + controller_interface geometry_msgs hardware_interface diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e88ef51cd2..9687f9dfee 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -32,14 +32,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() { try { - auto_declare("sensor_name", ""); - auto_declare("interface_names.force.x", ""); - auto_declare("interface_names.force.y", ""); - auto_declare("interface_names.force.z", ""); - auto_declare("interface_names.torque.x", ""); - auto_declare("interface_names.torque.y", ""); - auto_declare("interface_names.torque.z", ""); - auto_declare("frame_id", ""); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -53,18 +47,14 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); - interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string(); + params_ = param_listener_->get_params(); const bool no_interface_names_defined = - std::count(interface_names_.begin(), interface_names_.end(), "") == 6; + params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && + params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && + params_.interface_names.torque.y.empty() && params_.interface_names.torque.z.empty(); - if (sensor_name_.empty() && no_interface_names_defined) + if (params_.sensor_name.empty() && no_interface_names_defined) { RCLCPP_ERROR( get_node()->get_logger(), @@ -73,7 +63,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (!sensor_name_.empty() && !no_interface_names_defined) + if (!params_.sensor_name.empty() && !no_interface_names_defined) { RCLCPP_ERROR( get_node()->get_logger(), @@ -82,24 +72,25 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - frame_id_ = get_node()->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) + if (params_.frame_id.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return controller_interface::CallbackReturn::ERROR; } - if (!sensor_name_.empty()) + if (!params_.sensor_name.empty()) { force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(sensor_name_)); + semantic_components::ForceTorqueSensor(params_.sensor_name)); } else { + auto const & force_names = params_.interface_names.force; + auto const & torque_names = params_.interface_names.torque; force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor( - interface_names_[0], interface_names_[1], interface_names_[2], interface_names_[3], - interface_names_[4], interface_names_[5])); + force_names.x, force_names.y, force_names.z, torque_names.x, torque_names.y, + torque_names.z)); } try @@ -118,7 +109,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = frame_id_; + realtime_publisher_->msg_.header.frame_id = params_.frame_id; realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..059d5ab9a1 --- /dev/null +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -0,0 +1,44 @@ +force_torque_sensor_broadcaster: + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + interface_names: + force: + x: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'x' axis.", + } + y: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'y' axis.", + } + z: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'z' axis.", + } + torque: + x: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'x' axis.", + } + y: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'y' axis.", + } + z: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'z' axis.", + } diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 3a667e7d9a..6f9dc84a3f 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From fe89a549ac81d7baee4475a707ab3b0c869e1838 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 1 Sep 2022 01:39:46 -0500 Subject: [PATCH 141/524] Use a "steady clock" when measuring time differences (#427) * Use "steady" clocks when measuring changes in time * Check if the "publish rate" tests pass now --- .../test/test_trajectory.cpp | 7 +- .../test/test_trajectory_actions.cpp | 7 +- .../test/test_trajectory_controller.cpp | 77 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 7 +- 4 files changed, 50 insertions(+), 48 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 93e3560c07..fbf198300b 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -40,6 +40,7 @@ const double EPS = 1e-8; TEST(TestTrajectory, initialize_trajectory) { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); { auto empty_msg = std::make_shared(); empty_msg->header.stamp.sec = 2; @@ -49,7 +50,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -59,13 +60,13 @@ TEST(TestTrajectory, initialize_trajectory) auto empty_msg = std::make_shared(); empty_msg->header.stamp.sec = 0; empty_msg->header.stamp.nanosec = 0; - const auto now = rclcpp::Clock().now(); + const auto now = clock.now(); auto traj = joint_trajectory_controller::Trajectory(empty_msg); const auto traj_starttime = traj.time_from_start(); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 7128c0bee2..10dd3a6b90 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -91,12 +91,13 @@ class TestTrajectoryActions : public TrajectoryControllerTest [&]() { // controller hardware cycle update loop - auto start_time = rclcpp::Clock().now(); + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = clock.now(); rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) + while (clock.now() < end_time) { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } }); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index cc5b175281..e758dc038f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -343,7 +343,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // update for 0.25 seconds - const auto start_time = rclcpp::Clock().now(); updateController(rclcpp::Duration::from_seconds(0.25)); // should be home pose again @@ -489,52 +488,52 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } -// void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -// { -// rclcpp::Parameter state_publish_rate_param( -// "state_publish_rate", static_cast(target_msg_count)); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); +void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +{ + rclcpp::Parameter state_publish_rate_param( + "state_publish_rate", static_cast(target_msg_count)); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); -// auto future_handle = std::async( -// std::launch::async, [&executor]() -> void { executor.spin(); }); + auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); -// using control_msgs::msg::JointTrajectoryControllerState; + using control_msgs::msg::JointTrajectoryControllerState; -// const int qos_level = 10; -// int echo_received_counter = 0; -// rclcpp::Subscription::SharedPtr subs = -// traj_controller_->get_node()->create_subscription( -// controller_name_ + "/state", qos_level, -// [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + const int qos_level = 10; + int echo_received_counter = 0; + rclcpp::Subscription::SharedPtr subs = + traj_controller_->get_node()->create_subscription( + controller_name_ + "/state", qos_level, + [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); -// // update for 1second -// const auto start_time = rclcpp::Clock().now(); -// const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); -// const auto end_time = start_time + wait; -// while (rclcpp::Clock().now() < end_time) -// { -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// } + // update for 1second + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); + const auto end_time = start_time + wait; + while (clock.now() < end_time) + { + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + } -// // We may miss the last message since time allowed is exactly the time needed -// EXPECT_NEAR(target_msg_count, echo_received_counter, 1); + // We may miss the last message since time allowed is exactly the time needed + EXPECT_NEAR(target_msg_count, echo_received_counter, 1); -// executor.cancel(); -// } + executor.cancel(); +} -// /** -// * @brief test_state_publish_rate Test that state publish rate matches configure rate -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -// { -// test_state_publish_rate_target(10); -// } +/** + * @brief test_state_publish_rate Test that state publish rate matches configure rate + */ +TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +{ + test_state_publish_rate_target(10); +} -// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -// { -// test_state_publish_rate_target(0); -// } +TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +{ + test_state_publish_rate_target(0); +} // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index be11d274ad..840a624eca 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -349,11 +349,12 @@ class TrajectoryControllerTest : public ::testing::Test void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) { - const auto start_time = rclcpp::Clock().now(); + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); const auto end_time = start_time + wait_time; - while (rclcpp::Clock().now() < end_time) + while (clock.now() < end_time) { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } } From fe306da25f29cc44e3b1301b1301495dede8e058 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Thu, 1 Sep 2022 18:07:44 +0900 Subject: [PATCH 142/524] Add an initialization of the gripper action command for safe startup. (#425) --- .../gripper_controllers/gripper_action_controller_impl.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index ee07a9c0a0..6fb9d61359 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -298,6 +298,7 @@ controller_interface::CallbackReturn GripperActionController: // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = default_max_effort_; + command_.initRT(command_struct_); // Result pre_alloc_result_ = std::make_shared(); From dddab1dc7f63b028e83a4c99b55b0b36e647c8ef Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Sep 2022 10:27:29 +0100 Subject: [PATCH 143/524] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 12 ++++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 66 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5c40544d25..b2ac2dc2e3 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3d2babf2ed..4d1a88acf5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a3d5156859..c9f68ec19d 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate params for ForceTorqueSensorBroadcaster (`#395 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 9b1d48c875..d046dc0b5a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c06969c439..ccb4d7cefb 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add an initialization of the gripper action command for safe startup. (`#425 `_) +* Fix formatting CI job (`#418 `_) +* Contributors: Shota Aoki, Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 82c99bf60f..960f7a41d0 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3030632664..19fb75ae94 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- * Use explicit type in joint_state_broadcaster test (`#403 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index d435127b2f..a17cceb8ef 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use a "steady clock" when measuring time differences (`#427 `_) +* [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (`#380 `_) +* test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (`#415 `_) +* Reinstate JTC tests (`#391 `_) +* [JTC] Hold position if tolerance is violated even during non-active goal (`#368 `_) +* Small fixes for JTC. (`#390 `_) + variables in JTC to not clutter other PR with them. + fixes of updating parameters on renewed configuration of JTC that were missed +* Contributors: Andy Zelenak, Bence Magyar, Denis Štogl, Jaron Lundwall, Michael Wiznitzer + 2.11.0 (2022-08-04) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1affcdc4b8..14caad1b0a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index e53cadd749..ad8bd8a3ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- * Tricycle controller (`#345 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 7d319f9034..284e045a54 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 27bc6030cb..493ec46581 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: :bug: make bare exceptions more narrow (`#422 `_) +* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) +* Contributors: Denis Štogl, Jaron Lundwall + 2.11.0 (2022-08-04) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 7c929f4432..a15927cd1f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.11.0 (2022-08-04) ------------------- * Tricycle controller (`#345 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b9de02a603..cdbde1fb06 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- From 3425ebcd1293438c40596c1181fb2a5dbf815078 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Sep 2022 10:32:15 +0100 Subject: [PATCH 144/524] 2.12.0 --- 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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b2ac2dc2e3..b575411966 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Contributors: Tyler Weaver diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d82a4a2df8..1d17d25b58 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.11.0 + 2.12.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4d1a88acf5..6eca17389b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e716631d9e..692f1928f3 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index c9f68ec19d..f2ce7457eb 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Generate params for ForceTorqueSensorBroadcaster (`#395 `_) * Contributors: Tyler Weaver diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index de4a0e0513..11bb64374d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.11.0 + 2.12.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d046dc0b5a..5755b526e2 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 071701c5cd..73943f9d75 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ccb4d7cefb..bbf0f65e73 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Add an initialization of the gripper action command for safe startup. (`#425 `_) * Fix formatting CI job (`#418 `_) * Contributors: Shota Aoki, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a68306ee4e..6403d5efa7 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.11.0 + 2.12.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 960f7a41d0..ce5149b6c7 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index dec5c6b5e4..b6a28e765c 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.11.0 + 2.12.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 19fb75ae94..8fc784711b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Contributors: Tyler Weaver diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 12db7f0b2d..9262419ec4 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.11.0 + 2.12.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a17cceb8ef..967b3f5cee 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Use a "steady clock" when measuring time differences (`#427 `_) * [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (`#380 `_) * test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (`#415 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 00e6e9f6cb..158b399e09 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.11.0 + 2.12.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 14caad1b0a..9bdb24a322 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 3b7eb27975..2a4a3811d2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ad8bd8a3ca..4bf2fb4664 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 2298913120..e0436c2094 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.11.0 + 2.12.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 284e045a54..baa90562dc 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index a57a484e88..c714c93d1f 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.11.0 + 2.12.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 959ad0eb4d..3c3f49a876 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.11.0", + version="2.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 493ec46581..2da4d40515 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * fix: :bug: make bare exceptions more narrow (`#422 `_) * Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) * Contributors: Denis Štogl, Jaron Lundwall diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 70b8201345..48fe677f28 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.11.0 + 2.12.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index f5dc707899..40cc4abdd0 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.11.0", + version="2.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a15927cd1f..3875d2e729 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) * Contributors: Denis Štogl, Tyler Weaver diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 409ee6bc40..aa36f2822e 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.11.0 + 2.12.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cdbde1fb06..49ffeaee5b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index b1d500f1a1..a2cfa040bb 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From b31635a514d75a3ded928479b1f60781ed3b7be3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Sep 2022 18:33:35 +0200 Subject: [PATCH 145/524] Enable definition of all fields in JointTrajectory message when using test node. (#389) Co-authored-by: Bence Magyar --- .../publisher_joint_trajectory_controller.py | 103 +++++++++++++++--- 1 file changed, 86 insertions(+), 17 deletions(-) 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 77e04da356..301fe42cd4 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 @@ -30,9 +30,8 @@ def __init__(self): self.declare_parameter("controller_name", "position_trajectory_controller") self.declare_parameter("wait_sec_between_publish", 6) self.declare_parameter("goal_names", ["pos1", "pos2"]) - self.declare_parameter("joints") + self.declare_parameter("joints", [""]) self.declare_parameter("check_starting_point", False) - self.declare_parameter("starting_point_limits") # Read parameters controller_name = self.get_parameter("controller_name").value @@ -65,24 +64,96 @@ def __init__(self): self.joint_state_msg_received = False # Read all positions from parameters - self.goals = [] + self.goals = [] # List of JointTrajectoryPoint for name in goal_names: self.declare_parameter(name) goal = self.get_parameter(name).value - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') - float_goal = [] - for value in goal: - float_goal.append(float(value)) - self.goals.append(float_goal) + # TODO(anyone): remove this "if" part in ROS Iron + if isinstance(goal, list): + self.get_logger().warn( + f'Goal "{name}" is defined as a list. This is deprecated. ' + "Use the following structure:\n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) + + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [float(value) for value in goal] + + point = JointTrajectoryPoint() + point.positions = float_goal + point.time_from_start = Duration(sec=4) + + self.goals.append(point) + + else: + point = JointTrajectoryPoint() + + def get_sub_param(sub_param): + param_name = name + "." + sub_param + self.declare_parameter(param_name, [float()]) + param_value = self.get_parameter(param_name).value + + float_values = [] + + if len(param_value) != len(self.joints): + return [False, float_values] + + float_values = [float(value) for value in param_value] + + return [True, float_values] + + one_ok = False + + [ok, values] = get_sub_param("positions") + if ok: + point.positions = values + one_ok = True + + [ok, values] = get_sub_param("velocities") + if ok: + point.velocities = values + one_ok = True + + [ok, values] = get_sub_param("accelerations") + if ok: + point.accelerations = values + one_ok = True + + [ok, values] = get_sub_param("effort") + if ok: + point.effort = values + one_ok = True + + if one_ok: + point.time_from_start = Duration(sec=4) + self.goals.append(point) + self.get_logger().info(f'Goal "{name}" has definition {point}') + + else: + self.get_logger().warn( + f'Goal "{name}" definition is wrong. This goal will not be used. ' + "Use the following structure: \n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) + + if len(self.goals) < 1: + self.get_logger().error("No valid goal found. Exiting...") + exit(1) publish_topic = "/" + controller_name + "/" + "joint_trajectory" self.get_logger().info( - 'Publishing {} goals on topic "{}" every {} s'.format( - len(goal_names), publish_topic, wait_sec_between_publish - ) + f'Publishing {len(goal_names)} goals on topic "{publish_topic}" every ' + "{wait_sec_between_publish} s" ) self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) @@ -93,13 +164,11 @@ def timer_callback(self): if self.starting_point_ok: + self.get_logger().info(f"Sending goal {self.goals[self.i]}.") + traj = JointTrajectory() traj.joint_names = self.joints - point = JointTrajectoryPoint() - point.positions = self.goals[self.i] - point.time_from_start = Duration(sec=4) - - traj.points.append(point) + traj.points.append(self.goals[self.i]) self.publisher_.publish(traj) self.i += 1 From 0466d1d2105d182f400af7db2dd5e2c64af7318a Mon Sep 17 00:00:00 2001 From: Gilmar Correia Date: Tue, 6 Sep 2022 04:26:16 -0300 Subject: [PATCH 146/524] [JointStateBroadcaster] Reset internal variables to avoid duplication of joints (#431) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 18d5f25e60..c46d1fcd7a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -207,6 +207,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + joint_names_.clear(); + return CallbackReturn::SUCCESS; } @@ -229,6 +231,7 @@ bool has_any_key( bool JointStateBroadcaster::init_joint_data() { + joint_names_.clear(); if (state_interfaces_.empty()) { return false; From a9b9a9ca70e396537d8976f9944ec23311143aa2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 7 Sep 2022 19:38:42 +0200 Subject: [PATCH 147/524] Fix source builds (#430) --- .github/workflows/humble-source-build.yml | 2 +- .github/workflows/rolling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 6ab92814dc..18c33b6d52 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -16,4 +16,4 @@ jobs: with: ros_distro: humble ref: master - ros2_repo_branch: master-humble + ros2_repo_branch: humble diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 1a137a795b..40abcd1b0c 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -16,4 +16,4 @@ jobs: with: ros_distro: rolling ref: master - ros2_repo_branch: master-rolling + ros2_repo_branch: rolling From 96f1e597dc3ba25266270a1fc4f4cbbf0d187e47 Mon Sep 17 00:00:00 2001 From: Arshad Mehmood Date: Thu, 8 Sep 2022 01:44:49 +0800 Subject: [PATCH 148/524] Fix for high CPU usage by JTC in gzserver (#428) * Change type cast wall timer period from second to nanoseconds. create_wall_timer() expects delay in nanoseconds (duration object) however the type cast to seconds will result in 0 (if duration is less than 1s) and thus causing timer to be fired non stop resulting in very high CPU usage. * Reset smartpointer so that create_wall_timer() call can destroy previous trajectory timer. node->create_wall_timer() first removes timers associated with expired smartpointers before servicing current request. The JTC timer pointer gets overwrite only after the create_wall_timer() returns and thus not able to remove previous trajectory timer resulting in upto two timers running for JTC during trajectory execution. Althougth the previous timer does nothing but still get fired. Signed-off-by: Arshad Mehmood Signed-off-by: Arshad Mehmood --- .../src/joint_trajectory_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 64092e1ab5..f5f5190caa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1087,9 +1087,12 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), + action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } From 7b448c95a06d543f6e3cf80a9deddf7f3d2a5aaa Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 17 Sep 2022 14:23:58 +0800 Subject: [PATCH 149/524] Fix rates in JTC userdoc.rst (#433) --- joint_trajectory_controller/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 17c0c511c5..d225624096 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -121,16 +121,16 @@ state_interfaces (list(string)): Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. - Default: 50.0 - state_publish_rate (double): Publish-rate of the controller's "state" topic. - Default: 20.0 + Default: 50.0 action_monitor_rate (double): Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + Default: 20.0 + allow_partial_joints_goal (boolean): Allow joint goals defining trajectory for only some joints. From 9c81373c5ade476b854fdf971b3007939156c9f2 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Wed, 28 Sep 2022 04:24:22 -0500 Subject: [PATCH 150/524] ci: :construction_worker: update rhel container (#439) --- .github/workflows/galactic-rhel-binary-build.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml index 2116d5f4d5..5257ec0ee2 100644 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: galactic - container: jaronl/ros:galactic-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 42a700a7db..efa5ebbbb9 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble - container: jaronl/ros:humble-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index e85b439411..42adab88c7 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling - container: jaronl/ros:rolling-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: From 77a2c0658d89267eb7ee39a8aeb70d9bc39d89c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 28 Sep 2022 20:54:28 +0200 Subject: [PATCH 151/524] Fix undeclared and wrong parameters in controllers. (#438) * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. --- .../test/test_imu_sensor_broadcaster.cpp | 15 ++++++--------- .../src/joint_state_broadcaster.cpp | 1 + 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 42b53bf975..cc1398a9b8 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -102,7 +102,7 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & ASSERT_TRUE(subscription->take(imu_msg, msg_info)); } -TEST_F(IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) +TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) { SetUpIMUBroadcaster(); @@ -110,21 +110,18 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) +TEST_F(IMUSensorBroadcasterTest, SensorName_NotSet) { SetUpIMUBroadcaster(); - // set the 'interface_names' - imu_broadcaster_->get_node()->set_parameter( - {"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"}); - imu_broadcaster_->get_node()->set_parameter( - {"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"}); + // set the 'frame_id' + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - // configure failed, 'frame_id' parameter not set + // configure failed, 'sensor_name' parameter not set ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(IMUSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) +TEST_F(IMUSensorBroadcasterTest, FrameId_NotSet) { SetUpIMUBroadcaster(); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c46d1fcd7a..69f5441a6d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -53,6 +53,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_init() auto_declare("use_local_topics", false); auto_declare>("joints", std::vector({})); auto_declare>("interfaces", std::vector({})); + auto_declare>("extra_joints", std::vector({})); auto_declare( std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION); auto_declare( From 8d0345a37ee3cc8144a793cc7b464f42e4badf93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 1 Oct 2022 12:44:03 +0200 Subject: [PATCH 152/524] [CI] Add new variable to reusable workflow set unique name (#442) - Add new variable to reusable workflow to set unique name for the cache - Variable cannot be used in the container definition --- .github/workflows/galactic-rhel-binary-build.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/reusable-industrial-ci-with-cache.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml index 5257ec0ee2..518fcc186a 100644 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: galactic - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:galactic-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index efa5ebbbb9..8f4a65a9b5 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:humble-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 0ff359d091..490b680e7c 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -54,7 +54,7 @@ jobs: env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 42adab88c7..c418319cd7 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:rolling-rhel steps: - uses: actions/checkout@v3 with: From b157c0d35d42f850dbb48f1d35f91796344c2a5c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 1 Oct 2022 05:07:25 -0600 Subject: [PATCH 153/524] Generate Parameter Library for Joint Trajectory Controller (#384) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * generate_parameter_library for jtc Signed-off-by: Tyler Weaver * Update to latest generator Signed-off-by: Tyler Weaver * Use __map_joints feature Signed-off-by: Tyler Weaver * Update joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml Co-authored-by: AndyZe * Respond to feedback Signed-off-by: Tyler Weaver * Fix cppcheck errors Signed-off-by: Tyler Weaver * Add to repos file for CI Signed-off-by: Tyler Weaver * put ff_velocity_scale back to avoid changing the interface Signed-off-by: Tyler Weaver * update custom parameter validators Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver * command_interfaces may not be empty. * Make joint names unique and not empty. * Apply suggestions from code review * Apply suggestions from code review * Fixup the tests and finish the merge. * Fixup linters. * Fixup bug. * Use executor in all cases to avoid test timeouts on the CI. * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. * Update ros2_controllers.rolling.repos * Update ros2_controllers.humble.repos * Update ros2_controllers-not-released.humble.repos * Update ros2_controllers-not-released.rolling.repos * Update ros2_controllers.humble.repos * Update ros2_controllers-not-released.humble.repos * Update ros2_controllers-not-released.humble.repos * Remove using generate_parameters_library from source. Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver Co-authored-by: AndyZe Co-authored-by: Denis Štogl Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- joint_trajectory_controller/CMakeLists.txt | 8 + .../joint_trajectory_controller.hpp | 24 +- .../tolerances.hpp | 36 +- .../validate_jtc_parameters.hpp | 91 +++++ joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 328 +++++++----------- ...oint_trajectory_controller_parameters.yaml | 128 +++++++ .../test/test_trajectory_controller.cpp | 174 +++++----- .../test/test_trajectory_controller_utils.hpp | 36 +- 9 files changed, 476 insertions(+), 350 deletions(-) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp create mode 100644 joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5689e589f8..6d443c532f 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -29,11 +29,19 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(joint_trajectory_controller_parameters + src/joint_trajectory_controller_parameters.yaml + include/joint_trajectory_controller/validate_jtc_parameters.hpp +) + add_library(${PROJECT_NAME} SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE include) +target_link_libraries(${PROJECT_NAME} joint_trajectory_controller_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. 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 79fe6bccf4..a229489422 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 @@ -44,6 +44,8 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller_parameters.hpp" + using namespace std::chrono_literals; // NOLINT namespace rclcpp_action @@ -112,11 +114,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp_lifecycle::State & previous_state) override; protected: - std::vector joint_names_; - std::vector command_joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants const std::vector allowed_interface_types_ = { @@ -134,20 +131,18 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Degrees of freedom size_t dof_; - // Parameters for some special cases, e.g. hydraulics powered robots - // Run the controller in open-loop, i.e., read hardware states only when starting controller. - // This is useful when robot is not exactly following the commanded trajectory. - bool open_loop_control_ = false; + // Storing command joint names for interfaces + std::vector command_joint_names_; + + // Parameters from ROS for joint_trajectory_controller + std::shared_ptr param_listener_; + Params params_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - /// Allow integration in goal trajectories to accept goals without position or velocity specified - bool allow_integration_in_goal_trajectories_ = false; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; - double state_publish_rate_; - double action_monitor_rate_; - // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ @@ -201,7 +196,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; rclcpp_action::Server::SharedPtr action_server_; - bool allow_partial_joints_goal_ = false; RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8f3f30d476..de8f060b1a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -36,6 +36,7 @@ #include #include "control_msgs/action/follow_joint_trajectory.hpp" +#include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" @@ -87,45 +88,34 @@ struct SegmentTolerances * goal: 0.01 * \endcode * - * \param node LifecycleNode where the tolerances are specified. - * \param joint_names Names of joints to look for in the parameter server for a tolerance specification. + * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances( - const rclcpp_lifecycle::LifecycleNode & node, const std::vector & joint_names) +SegmentTolerances get_segment_tolerances(Params const & params) { - const auto n_joints = joint_names.size(); + auto const & constraints = params.constraints; + auto const n_joints = params.joints.size(); + SegmentTolerances tolerances; + tolerances.goal_time_tolerance = constraints.goal_time; // State and goal state tolerances - double stopped_velocity_tolerance = 0.01; - node.get_parameter_or( - "constraints.stopped_velocity_tolerance", stopped_velocity_tolerance, - stopped_velocity_tolerance); - tolerances.state_tolerance.resize(n_joints); tolerances.goal_state_tolerance.resize(n_joints); for (size_t i = 0; i < n_joints; ++i) { - const std::string prefix = "constraints." + joint_names[i]; - - node.get_parameter_or( - prefix + ".trajectory", tolerances.state_tolerance[i].position, 0.0); - node.get_parameter_or( - prefix + ".goal", tolerances.goal_state_tolerance[i].position, 0.0); + auto const joint = params.joints[i]; + tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; + tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; + tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); - - tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance; + logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); } - // Goal time tolerance - node.get_parameter_or("constraints.goal_time", tolerances.goal_time_tolerance, 0.0); - return tolerances; } 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 new file mode 100644 index 0000000000..09363eebb1 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ +Result command_interface_type_combinations(rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + if ( + contains(interface_types, "velocity") && interface_types.size() > 1 && + !contains(interface_types, "position")) + { + return ERROR( + "'velocity' command interface can be used either alone or 'position' " + "interface has to be present"); + } + + if ( + contains(interface_types, "acceleration") && + (!contains(interface_types, "velocity") && + !contains(interface_types, "position"))) + { + return ERROR( + "'acceleration' command interface can only be used if 'velocity' and " + "'position' interfaces are present"); + } + + if (contains(interface_types, "effort") && interface_types.size() > 1) + { + return ERROR("'effort' command interface has to be used alone"); + } + + return OK; +} + +Result state_interface_type_combinations(rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Valid combinations are + // 1. position [velocity, [acceleration]] + + if ( + contains(interface_types, "velocity") && + !contains(interface_types, "position")) + { + return ERROR( + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + } + + if ( + contains(interface_types, "acceleration") && + (!contains(interface_types, "position") || + !contains(interface_types, "velocity"))) + { + return ERROR( + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + } + + return OK; +} + +} // namespace parameter_traits + +#endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 158b399e09..3880b28508 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -12,6 +12,7 @@ ament_cmake angles + generate_parameter_library pluginlib realtime_tools diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f5f5190caa..96110adcb2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,7 +46,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}), dof_(0) +: controller_interface::ControllerInterface(), dof_(0) { } @@ -54,26 +54,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - joint_names_ = auto_declare>("joints", joint_names_); - command_joint_names_ = - auto_declare>("command_joints", command_joint_names_); - command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); - state_interface_types_ = - auto_declare>("state_interfaces", state_interface_types_); - allow_partial_joints_goal_ = - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); - allow_integration_in_goal_trajectories_ = auto_declare( - "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); - state_publish_rate_ = auto_declare("state_publish_rate", 50.0); - action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); - - const std::string interpolation_string = auto_declare( - "interpolation_method", interpolation_methods::InterpolationMethodMap.at( - interpolation_methods::DEFAULT_INTERPOLATION)); - interpolation_method_ = interpolation_methods::from_string(interpolation_string); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + // Set interpolation method from string parameter + interpolation_method_ = interpolation_methods::from_string(params_.interpolation_method); } catch (const std::exception & e) { @@ -98,10 +84,10 @@ JointTrajectoryController::command_interface_configuration() const dof_); std::exit(EXIT_FAILURE); } - conf.names.reserve(dof_ * command_interface_types_.size()); + conf.names.reserve(dof_ * params_.command_interfaces.size()); for (const auto & joint_name : command_joint_names_) { - for (const auto & interface_type : command_interface_types_) + for (const auto & interface_type : params_.command_interfaces) { conf.names.push_back(joint_name + "/" + interface_type); } @@ -114,10 +100,10 @@ JointTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(dof_ * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) + conf.names.reserve(dof_ * params_.state_interfaces.size()); + for (const auto & joint_name : params_.joints) { - for (const auto & interface_type : state_interface_types_) + for (const auto & interface_type : params_.state_interfaces) { conf.names.push_back(joint_name + "/" + interface_type); } @@ -185,7 +171,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!(*traj_point_active_ptr_)->is_sampled_already()) { first_sample = true; - if (open_loop_control_) + if (params_.open_loop_control) { (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); } @@ -305,7 +291,7 @@ controller_interface::return_type JointTrajectoryController::update( // send feedback auto feedback = std::make_shared(); feedback->header.stamp = time; - feedback->joint_names = joint_names_; + feedback->joint_names = params_.joints; feedback->actual = state_current_; feedback->desired = state_desired_; @@ -481,9 +467,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { const auto logger = get_node()->get_logger(); - // update parameters - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - if ((dof_ > 0) && (joint_names_.size() != dof_)) + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + + // 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(); + + // Check if the DoF has changed + if ((dof_ > 0) && (params_.joints.size() != dof_)) { RCLCPP_ERROR( logger, @@ -493,7 +490,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - dof_ = joint_names_.size(); + dof_ = params_.joints.size(); // TODO(destogl): why is this here? Add comment or move if (!reset()) @@ -501,34 +498,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - if (joint_names_.empty()) + if (params_.joints.empty()) { // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } - command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + command_joint_names_ = params_.command_joints; if (command_joint_names_.empty()) { - command_joint_names_ = joint_names_; + command_joint_names_ = params_.joints; RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); } - else if (command_joint_names_.size() != joint_names_.size()) + else if (command_joint_names_.size() != params_.joints.size()) { RCLCPP_ERROR( logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); 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(); - } + // // 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 (command_interface_types_.empty()) + if (params_.command_interfaces.empty()) { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); return CallbackReturn::FAILURE; @@ -537,70 +534,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } - - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] has_position_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_POSITION); has_velocity_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_VELOCITY); has_acceleration_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); has_effort_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); - - if (has_velocity_command_interface_) - { - // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else if (!has_position_command_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present."); - return CallbackReturn::FAILURE; - } - // invalid: acceleration is defined and no velocity - } - else if (has_acceleration_command_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); - return CallbackReturn::FAILURE; - } + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); - // effort can't be combined with other interfaces - if (has_effort_command_interface_) - { - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else - { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); - return CallbackReturn::FAILURE; - } - } + // if there is only velocity or if there is effort command interface + // then use also PID adapter + use_closed_loop_pid_adapter_ = + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || + has_effort_command_interface_; if (use_closed_loop_pid_adapter_) { @@ -608,35 +556,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameter server - for (size_t i = 0; i < pids_.size(); ++i) + // Init PID gains from ROS parameters + for (size_t i = 0; i < dof_; ++i) { - const std::string prefix = "gains." + command_joint_names_[i]; - const auto k_p = auto_declare(prefix + ".p", 0.0); - const auto k_i = auto_declare(prefix + ".i", 0.0); - const auto k_d = auto_declare(prefix + ".d", 0.0); - const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = - auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); - // Initialize PID - pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); - } - } + 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); - // Read always state interfaces from the parameter because they can be used - // independently from the controller's type. - // Specialized, child controllers should set its default value. - state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); + // TODO(destogl): remove this in ROS2 Iron + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - if (state_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) + if (params_.state_interfaces.empty()) { - RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); return CallbackReturn::FAILURE; } @@ -644,62 +591,38 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // allocation during activation // Note: 'effort' storage is also here, but never used. Still, for this is OK. joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } has_position_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_POSITION); has_velocity_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_VELOCITY); has_acceleration_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); - if (has_velocity_state_interface_) + // Validation of combinations of state and velocity together have to be done + // here because the parameter validators only deal with each parameter + // separately. + if ( + has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + (!has_velocity_state_interface_ || !has_position_state_interface_)) { - if (!has_position_state_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - return CallbackReturn::FAILURE; - } + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; } - else + + // effort is always used alone so no need for size check + if ( + has_effort_command_interface_ && + (!has_velocity_state_interface_ || !has_position_state_interface_)) { - if (has_acceleration_state_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; - } - if (has_velocity_command_interface_ && command_interface_types_.size() == 1) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } - // effort is always used alone so no need for size check - if (has_effort_command_interface_) - { - RCLCPP_ERROR( - logger, - "'effort' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; } auto get_interface_list = [](const std::vector & interface_types) @@ -719,15 +642,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Print output so users can be sure the interface setup is correct RCLCPP_INFO( logger, "Command interfaces are [%s] and state interfaces are [%s].", - get_interface_list(command_interface_types_).c_str(), - get_interface_list(state_interface_types_).c_str()); - - default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); + get_interface_list(params_.command_interfaces).c_str(), + get_interface_list(params_.state_interfaces).c_str()); - // Read parameters customizing controller for special cases - open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); - allow_integration_in_goal_trajectories_ = - get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + default_tolerances_ = get_segment_tolerances(params_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); @@ -742,11 +660,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher - state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); - if (state_publish_rate_ > 0.0) + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", params_.state_publish_rate); + if (params_.state_publish_rate > 0.0) { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / params_.state_publish_rate); } else { @@ -758,7 +675,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); - state_publisher_->msg_.joint_names = command_joint_names_; + state_publisher_->msg_.joint_names = params_.joints; state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); @@ -779,16 +696,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( last_state_publish_time_ = get_node()->now(); // action server configuration - allow_partial_joints_goal_ = - get_node()->get_parameter("allow_partial_joints_goal").get_value(); - if (allow_partial_joints_goal_) + if (params_.allow_partial_joints_goal) { RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } - action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); + RCLCPP_INFO( + logger, "Action status changes will be monitored at %.2f Hz.", params_.action_monitor_rate); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( @@ -811,7 +726,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { // order all joints in the storage - for (const auto & interface : command_interface_types_) + for (const auto & interface : params_.command_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); @@ -825,13 +740,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( return CallbackReturn::ERROR; } } - for (const auto & interface : state_interface_types_) + for (const auto & interface : params_.state_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, @@ -943,7 +858,10 @@ bool JointTrajectoryController::reset() for (const auto & pid : pids_) { - pid->reset(); + if (pid) + { + pid->reset(); + } } // iterator has no default value @@ -953,12 +871,6 @@ bool JointTrajectoryController::reset() traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); - // reset pids - for (const auto & pid : pids_) - { - pid->reset(); - } - return true; } @@ -1083,7 +995,7 @@ void JointTrajectoryController::goal_accepted_callback( // Update the active goal RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); - rt_goal->preallocated_feedback_->joint_names = joint_names_; + rt_goal->preallocated_feedback_->joint_names = params_.joints; rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); @@ -1114,12 +1026,12 @@ void JointTrajectoryController::fill_partial_goal( if ( std::find( trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - joint_names_[index]) != trajectory_msg->joint_names.end()) + params_.joints[index]) != trajectory_msg->joint_names.end()) { // joint found on msg continue; } - trajectory_msg->joint_names.push_back(joint_names_[index]); + trajectory_msg->joint_names.push_back(params_.joints[index]); for (auto & it : trajectory_msg->points) { @@ -1160,7 +1072,7 @@ void JointTrajectoryController::sort_to_local_joint_order( std::shared_ptr trajectory_msg) { // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); + std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); auto remap = [this]( const std::vector & to_remap, const std::vector & mapping) -> std::vector @@ -1223,7 +1135,7 @@ bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) + if (!params_.allow_partial_joints_goal) { if (trajectory.joint_names.size() != dof_) { @@ -1265,8 +1177,8 @@ bool JointTrajectoryController::validate_trajectory_msg( { const std::string & incoming_joint_name = trajectory.joint_names[i]; - auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) + auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name); + if (it == params_.joints.end()) { RCLCPP_ERROR( get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", @@ -1292,7 +1204,7 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; // This currently supports only position, velocity and acceleration inputs - if (allow_integration_in_goal_trajectories_) + if (params_.allow_integration_in_goal_trajectories) { const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && points[i].accelerations.empty(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml new file mode 100644 index 0000000000..80aa32585b --- /dev/null +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -0,0 +1,128 @@ +joint_trajectory_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + validation: { + unique<>: null, + } + } + command_joints: { + type: string_array, + default_value: [], + description: "Names of the commanding joints used by the controller", + validation: { + unique<>: null, + } + } + command_interfaces: { + type: string_array, + default_value: [], + description: "Names of command interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration", "effort",]], + command_interface_type_combinations: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "Names of state interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration",]], + state_interface_type_combinations: null, + } + } + allow_partial_joints_goal: { + type: bool, + default_value: false, + description: "Goals with partial set of joints are allowed", + } + open_loop_control: { + type: bool, + default_value: false, + description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + } + allow_integration_in_goal_trajectories: { + type: bool, + default_value: false, + description: "Allow integration in goal trajectories to accept goals without position or velocity specified", + } + state_publish_rate: { + type: double, + default_value: 50.0, + description: "Rate controller state is published", + validation: { + lower_bounds: [0.1] + } + } + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Rate status changes will be monitored", + validation: { + lower_bounds: [0.1] + } + } + interpolation_method: { + type: string, + default_value: "splines", + description: "The type of interpolation to use, if any", + validation: { + one_of<>: [["splines", "none"]], + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Intigral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integrale clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling of velocity." + } + constraints: + stopped_velocity_tolerance: { + type: double, + default_value: 0.01, + description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.", + } + goal_time: { + type: double, + default_value: 0.0, + description: "Time tolerance for achieving trajectory goal before or after commanded time.", + validation: { + lower_bounds: [0.0], + } + } + __map_joints: + trajectory: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance during movement.", + } + goal: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance at the goal position.", + } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e758dc038f..6b0020c294 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -63,10 +63,9 @@ void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { - SetUpTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpTrajectoryController(executor); + // const auto future_handle_ = std::async(std::launch::async, spin, &executor); const auto state = traj_controller_->get_node()->configure(); @@ -97,7 +96,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) TEST_P(TrajectoryControllerTestParameterized, check_interface_names) { - SetUpTrajectoryController(); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -135,7 +135,8 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { - SetUpTrajectoryController(); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); // set command_joints parameter const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); @@ -178,10 +179,8 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command TEST_P(TrajectoryControllerTestParameterized, activate) { - SetUpTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpTrajectoryController(executor); traj_controller_->get_node()->configure(); ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); @@ -315,11 +314,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { - SetUpAndActivateTrajectoryController(); - - auto traj_node = traj_controller_->get_node(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_node->get_node_base_interface()); + SetUpAndActivateTrajectoryController(executor); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -355,7 +351,8 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { - SetUpTrajectoryController(false); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor, false); // This call is replacing the way parameters are set via launch SetParameters(); @@ -364,8 +361,6 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -439,7 +434,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); + SetUpAndActivateTrajectoryController(executor, true, {}); subscribeToState(); updateController(); @@ -493,7 +488,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou rclcpp::Parameter state_publish_rate_param( "state_publish_rate", static_cast(target_msg_count)); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); + SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); @@ -530,10 +525,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) test_state_publish_rate_target(10); } -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -{ - test_state_publish_rate_target(0); -} +// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +// { +// test_state_publish_rate_target(0); +// } // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from @@ -1393,71 +1388,72 @@ INSTANTIATE_TEST_SUITE_P( // std::vector({"effort"}), // std::vector({"position", "velocity", "acceleration"})))); -TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -{ - auto set_parameter_and_check_result = [&]() - { - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - SetParameters(); // This call is replacing the way parameters are set via launch - traj_controller_->get_node()->configure(); - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - }; - - SetUpTrajectoryController(false); - - // command interfaces: empty - command_interface_types_ = {}; - set_parameter_and_check_result(); - - // command interfaces: bad_name - command_interface_types_ = {"bad_name"}; - set_parameter_and_check_result(); - - // command interfaces: effort has to be only - command_interface_types_ = {"effort", "position"}; - set_parameter_and_check_result(); - - // command interfaces: velocity - position not present - command_interface_types_ = {"velocity", "acceleration"}; - set_parameter_and_check_result(); - - // command interfaces: acceleration without position and velocity - command_interface_types_ = {"acceleration"}; - set_parameter_and_check_result(); - - // state interfaces: empty - state_interface_types_ = {}; - set_parameter_and_check_result(); - - // state interfaces: cannot not be effort - state_interface_types_ = {"effort"}; - set_parameter_and_check_result(); - - // state interfaces: bad name - state_interface_types_ = {"bad_name"}; - set_parameter_and_check_result(); - - // state interfaces: velocity - position not present - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity", "acceleration"}; - set_parameter_and_check_result(); - - // state interfaces: acceleration without position and velocity - state_interface_types_ = {"acceleration"}; - set_parameter_and_check_result(); - - // velocity-only command interface: position - velocity not present - command_interface_types_ = {"velocity"}; - state_interface_types_ = {"position"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - - // effort-only command interface: position - velocity not present - command_interface_types_ = {"effort"}; - state_interface_types_ = {"position"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); -} +// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` +// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +// { +// auto set_parameter_and_check_result = [&]() +// { +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// SetParameters(); // This call is replacing the way parameters are set via launch +// traj_controller_->get_node()->configure(); +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// }; +// +// SetUpTrajectoryController(false); +// +// // command interfaces: empty +// command_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // command interfaces: bad_name +// command_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // command interfaces: effort has to be only +// command_interface_types_ = {"effort", "position"}; +// set_parameter_and_check_result(); +// +// // command interfaces: velocity - position not present +// command_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // command interfaces: acceleration without position and velocity +// command_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: empty +// state_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // state interfaces: cannot not be effort +// state_interface_types_ = {"effort"}; +// set_parameter_and_check_result(); +// +// // state interfaces: bad name +// state_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // state interfaces: velocity - position not present +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: acceleration without position and velocity +// state_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // velocity-only command interface: position - velocity not present +// command_interface_types_ = {"velocity"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// +// // effort-only command interface: position - velocity not present +// command_interface_types_ = {"effort"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 840a624eca..0d523cc88d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -79,7 +79,10 @@ class TestableJointTrajectoryController return success; } - void set_joint_names(const std::vector & joint_names) { joint_names_ = joint_names; } + void set_joint_names(const std::vector & joint_names) + { + params_.joints = joint_names; + } void set_command_joint_names(const std::vector & command_joint_names) { @@ -88,14 +91,16 @@ class TestableJointTrajectoryController void set_command_interfaces(const std::vector & command_interfaces) { - command_interface_types_ = command_interfaces; + params_.command_interfaces = command_interfaces; } void set_state_interfaces(const std::vector & state_interfaces) { - state_interface_types_ = state_interfaces; + params_.state_interfaces = state_interfaces; } + void trigger_declare_parameters() { param_listener_->declare_params(); } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() { return last_commanded_state_; @@ -138,9 +143,10 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); } - void SetUpTrajectoryController(bool use_local_parameters = true) + void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) { traj_controller_ = std::make_shared(); + if (use_local_parameters) { traj_controller_->set_joint_names(joint_names_); @@ -152,6 +158,8 @@ class TrajectoryControllerTest : public ::testing::Test { FAIL(); } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetParameters(); } void SetParameters() @@ -162,8 +170,10 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } + void SetPidParameters() { + traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); for (size_t i = 0; i < joint_names_.size(); ++i) @@ -173,32 +183,28 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale("ff_velocity_scale/" + joint_names_[i], 1.0); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } void SetUpAndActivateTrajectoryController( - bool use_local_parameters = true, const std::vector & parameters = {}, - rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) + rclcpp::Executor & executor, bool use_local_parameters = true, + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false) { - SetUpTrajectoryController(use_local_parameters); + SetUpTrajectoryController(executor, use_local_parameters); + // set pid parameters before configure + SetPidParameters(); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); } - if (executor) - { - executor->add_node(traj_controller_->get_node()->get_node_base_interface()); - } - // 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); - // set pid parameters before configure - SetPidParameters(); traj_controller_->get_node()->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); From 133ac357b3b539b1c45c246bd56b0b99b151177b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 5 Oct 2022 00:03:14 -0600 Subject: [PATCH 154/524] Generate parameters for Joint State Broadcaster (#401) Co-authored-by: Bence Magyar --- joint_state_broadcaster/CMakeLists.txt | 10 ++++ .../joint_state_broadcaster.hpp | 6 +-- joint_state_broadcaster/package.xml | 3 +- .../src/joint_state_broadcaster.cpp | 53 ++++++++----------- .../joint_state_broadcaster_parameters.yaml | 30 +++++++++++ 5 files changed, 68 insertions(+), 34 deletions(-) create mode 100644 joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index f8c14de1ef..0717577e15 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -14,12 +14,17 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(sensor_msgs REQUIRED) +generate_parameter_library(joint_state_broadcaster_parameters + src/joint_state_broadcaster_parameters.yaml +) + add_library(joint_state_broadcaster SHARED src/joint_state_broadcaster.cpp @@ -29,12 +34,16 @@ ament_target_dependencies(joint_state_broadcaster builtin_interfaces control_msgs controller_interface + generate_parameter_library pluginlib rclcpp_lifecycle rcutils realtime_tools sensor_msgs ) +target_link_libraries(joint_state_broadcaster + joint_state_broadcaster_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") @@ -84,6 +93,7 @@ endif() ament_export_dependencies( control_msgs controller_interface + generate_parameter_library rclcpp_lifecycle sensor_msgs ) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index f3716149a5..4761f3f250 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -23,6 +23,7 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "joint_state_broadcaster/visibility_control.h" +#include "joint_state_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" @@ -93,9 +94,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface protected: // Optional parameters - bool use_local_topics_; - std::vector joints_; - std::vector interfaces_; + std::shared_ptr param_listener_; + Params params_; std::unordered_map map_interface_to_joint_state_; // For the JointState message, diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9262419ec4..fea91eee4d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -20,10 +20,11 @@ control_msgs controller_interface + generate_parameter_library hardware_interface rclcpp_lifecycle - sensor_msgs realtime_tools + sensor_msgs ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 69f5441a6d..74a19c9ed0 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -50,16 +50,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_init() { try { - auto_declare("use_local_topics", false); - auto_declare>("joints", std::vector({})); - auto_declare>("interfaces", std::vector({})); - auto_declare>("extra_joints", std::vector({})); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, HW_IF_VELOCITY); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_EFFORT, HW_IF_EFFORT); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -89,9 +81,9 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf else { state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint : joints_) + for (const auto & joint : params_.joints) { - for (const auto & interface : interfaces_) + for (const auto & interface : params_.interfaces) { state_interfaces_config.names.push_back(joint + "/" + interface); } @@ -104,9 +96,12 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf controller_interface::CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); - joints_ = get_node()->get_parameter("joints").as_string_array(); - interfaces_ = get_node()->get_parameter("interfaces").as_string_array(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); if (use_all_available_interfaces()) { @@ -114,8 +109,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( get_node()->get_logger(), "'joints' or 'interfaces' parameter is empty. " "All available state interfaces will be published"); - joints_.clear(); - interfaces_.clear(); + params_.joints.clear(); + params_.interfaces.clear(); } else { @@ -124,14 +119,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); } - auto get_map_interface_parameter = [&](const std::string & interface) + auto get_map_interface_parameter = + [&](std::string const & interface, std::string const & interface_to_map) { - std::string interface_to_map = - get_node() - ->get_parameter(std::string("map_interface_to_joint_state.") + interface) - .as_string(); - - if (std::find(interfaces_.begin(), interfaces_.end(), interface) != interfaces_.end()) + if ( + std::find(params_.interfaces.begin(), params_.interfaces.end(), interface) != + params_.interfaces.end()) { map_interface_to_joint_state_[interface] = interface; RCLCPP_WARN( @@ -147,13 +140,13 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( }; map_interface_to_joint_state_ = {}; - get_map_interface_parameter(HW_IF_POSITION); - get_map_interface_parameter(HW_IF_VELOCITY); - get_map_interface_parameter(HW_IF_EFFORT); + get_map_interface_parameter(HW_IF_POSITION, params_.map_interface_to_joint_state.position); + get_map_interface_parameter(HW_IF_VELOCITY, params_.map_interface_to_joint_state.velocity); + get_map_interface_parameter(HW_IF_EFFORT, params_.map_interface_to_joint_state.effort); try { - const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); @@ -194,7 +187,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( if ( !use_all_available_interfaces() && - state_interfaces_.size() != (joints_.size() * interfaces_.size())) + state_interfaces_.size() != (params_.joints.size() * params_.interfaces.size())) { RCLCPP_WARN( get_node()->get_logger(), @@ -321,7 +314,7 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg() bool JointStateBroadcaster::use_all_available_interfaces() const { - return joints_.empty() || interfaces_.empty(); + return params_.joints.empty() || params_.interfaces.empty(); } double get_value( diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml new file mode 100644 index 0000000000..ba0d4f0051 --- /dev/null +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -0,0 +1,30 @@ +joint_state_broadcaster: + use_local_topics: { + type: bool, + default_value: false, + } + joints: { + type: string_array, + default_value: [], + } + extra_joints: { + type: string_array, + default_value: [], + } + interfaces: { + type: string_array, + default_value: [], + } + map_interface_to_joint_state: + position: { + type: string, + default_value: "position", + } + velocity: { + type: string, + default_value: "velocity", + } + effort: { + type: string, + default_value: "effort", + } From e0df7ddc5dd6f5800a03d994ef122315b9519c00 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Oct 2022 07:06:06 +0100 Subject: [PATCH 155/524] Update changelogs --- 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 | 7 +++++++ joint_state_broadcaster/CHANGELOG.rst | 9 +++++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 62 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b575411966..295549da24 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6eca17389b..a99f7e090b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index f2ce7457eb..e3bc6ca2ba 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Generate params for ForceTorqueSensorBroadcaster (`#395 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 5755b526e2..6ef0b4bc7d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bbf0f65e73..f53c2718d2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Add an initialization of the gripper action command for safe startup. (`#425 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ce5149b6c7..42408c5560 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix undeclared and wrong parameters in controllers. (`#438 `_) + * Add missing parameter declaration in the joint state broadcaster. + * Fix unsensible test in IMU Sensor Broadcaster. +* Contributors: Denis Štogl + 2.12.0 (2022-09-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8fc784711b..920b585dcf 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate parameters for Joint State Broadcaster (`#401 `_) +* Fix undeclared and wrong parameters in controllers. (`#438 `_) + * Add missing parameter declaration in the joint state broadcaster. + * Fix unsensible test in IMU Sensor Broadcaster. +* [JointStateBroadcaster] Reset internal variables to avoid duplication of joints (`#431 `_) +* Contributors: Denis Štogl, Gilmar Correia, Tyler Weaver, Bence Magyar + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 967b3f5cee..1a077eff84 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate Parameter Library for Joint Trajectory Controller (`#384 `_) +* Fix rates in JTC userdoc.rst (`#433 `_) +* Fix for high CPU usage by JTC in gzserver (`#428 `_) + * Change type cast wall timer period from second to nanoseconds. + create_wall_timer() expects delay in nanoseconds (duration object) however the type cast to seconds will result in 0 (if duration is less than 1s) and thus causing timer to be fired non stop resulting in very high CPU usage. + * Reset smartpointer so that create_wall_timer() call can destroy previous trajectory timer. + node->create_wall_timer() first removes timers associated with expired smartpointers before servicing current request. The JTC timer pointer gets overwrite only after the create_wall_timer() returns and thus not able to remove previous trajectory timer resulting in upto two timers running for JTC during trajectory execution. Althougth the previous timer does nothing but still get fired. +* Contributors: Arshad Mehmood, Borong Yuan, Tyler Weaver, Andy Zelenak, Bence Magyar, Denis Štogl + 2.12.0 (2022-09-01) ------------------- * Use a "steady clock" when measuring time differences (`#427 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 9bdb24a322..d270071f5e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4bf2fb4664..06a7f4327d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index baa90562dc..e413d8ee5a 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 +----------- +* Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) +* Contributors: Denis Štogl + 2.12.0 (2022-09-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 2da4d40515..95cc41f1d0 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * fix: :bug: make bare exceptions more narrow (`#422 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3875d2e729..aef811442a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 49ffeaee5b..b9b5d6f569 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- From 2457680a8e0dcb22cbb48361783e456fef681c30 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Oct 2022 07:06:29 +0100 Subject: [PATCH 156/524] 2.13.0 --- 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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 295549da24..3c2714dc64 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 1d17d25b58..474ff1a2d8 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.12.0 + 2.13.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 a99f7e090b..aeec4541f4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 692f1928f3..f9164998db 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.12.0 + 2.13.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 e3bc6ca2ba..7e285a6ac0 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 11bb64374d..bf1e01bde7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.12.0 + 2.13.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 6ef0b4bc7d..19b4711223 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 73943f9d75..f4c137301a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f53c2718d2..ee226c598f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 6403d5efa7..1f5ae7ceba 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.12.0 + 2.13.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 42408c5560..285d7e061d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Fix undeclared and wrong parameters in controllers. (`#438 `_) * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b6a28e765c..bc9d930830 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.12.0 + 2.13.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 920b585dcf..9dc2a70a9d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Generate parameters for Joint State Broadcaster (`#401 `_) * Fix undeclared and wrong parameters in controllers. (`#438 `_) * Add missing parameter declaration in the joint state broadcaster. diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fea91eee4d..43687c70ea 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.12.0 + 2.13.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 1a077eff84..f7244e062d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Generate Parameter Library for Joint Trajectory Controller (`#384 `_) * Fix rates in JTC userdoc.rst (`#433 `_) * Fix for high CPU usage by JTC in gzserver (`#428 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3880b28508..b3a828efe2 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.12.0 + 2.13.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 d270071f5e..d78bfc246b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2a4a3811d2..02d9ca7cf4 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 06a7f4327d..0c194f3db8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e0436c2094..125988addf 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.12.0 + 2.13.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 e413d8ee5a..b791ac2a81 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c714c93d1f..a3810d560a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.12.0 + 2.13.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 3c3f49a876..fdc3646ad1 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.12.0", + version="2.13.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 95cc41f1d0..130d482684 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 48fe677f28..7529c38428 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.12.0 + 2.13.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 40cc4abdd0..d1a720d311 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.12.0", + version="2.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index aef811442a..c71becc1ce 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index aa36f2822e..467ed03ffc 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.12.0 + 2.13.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b9b5d6f569..cdabd8ca5e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index a2cfa040bb..aecb6cf3d1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 34da157213e64dbccbc32f3d478519371dbac54c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 8 Oct 2022 18:31:39 +0200 Subject: [PATCH 157/524] Remove deprecation warning when parameter without value is set. (#445) --- .../publisher_joint_trajectory_controller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 301fe42cd4..56b56c5998 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 @@ -18,6 +18,7 @@ import rclpy from rclpy.node import Node from builtin_interfaces.msg import Duration +from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -66,7 +67,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] # List of JointTrajectoryPoint for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) goal = self.get_parameter(name).value # TODO(anyone): remove this "if" part in ROS Iron From 9be3bfe2614230606ce02b95aabb604e616fd089 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 10 Oct 2022 17:43:06 -0400 Subject: [PATCH 158/524] Add generic admittance controller for TCP wrenches (#370) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: AndyZe Co-authored-by: Denis Štogl --- admittance_controller/CMakeLists.txt | 122 ++++ .../admittance_controller.xml | 9 + admittance_controller/doc/userdoc.rst | 58 ++ .../admittance_controller.hpp | 187 ++++++ .../admittance_controller/admittance_rule.hpp | 216 +++++++ .../admittance_rule_impl.hpp | 405 ++++++++++++ .../visibility_control.h | 49 ++ admittance_controller/package.xml | 43 ++ .../src/admittance_controller.cpp | 592 ++++++++++++++++++ .../src/admittance_controller_parameters.yaml | 159 +++++ .../test/6d_robot_description.hpp | 313 +++++++++ .../test/test_admittance_controller.cpp | 292 +++++++++ .../test/test_admittance_controller.hpp | 463 ++++++++++++++ .../test/test_load_admittance_controller.cpp | 47 ++ admittance_controller/test/test_params.yaml | 111 ++++ ros2_controllers-not-released.humble.repos | 4 + ros2_controllers-not-released.rolling.repos | 4 + ros2_controllers.foxy.repos | 4 - ros2_controllers.galactic.repos | 4 - ros2_controllers.humble.repos | 16 +- ros2_controllers.rolling.repos | 19 +- 21 files changed, 3098 insertions(+), 19 deletions(-) create mode 100644 admittance_controller/CMakeLists.txt create mode 100644 admittance_controller/admittance_controller.xml create mode 100644 admittance_controller/doc/userdoc.rst create mode 100644 admittance_controller/include/admittance_controller/admittance_controller.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule_impl.hpp create mode 100644 admittance_controller/include/admittance_controller/visibility_control.h create mode 100644 admittance_controller/package.xml create mode 100644 admittance_controller/src/admittance_controller.cpp create mode 100644 admittance_controller/src/admittance_controller_parameters.yaml create mode 100644 admittance_controller/test/6d_robot_description.hpp create mode 100644 admittance_controller/test/test_admittance_controller.cpp create mode 100644 admittance_controller/test/test_admittance_controller.hpp create mode 100644 admittance_controller/test/test_load_admittance_controller.cpp create mode 100644 admittance_controller/test/test_params.yaml diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt new file mode 100644 index 0000000000..bbfe19deda --- /dev/null +++ b/admittance_controller/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5) +project(admittance_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + kinematics_interface + Eigen3 + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) +target_include_directories(${PROJECT_NAME} PRIVATE include) +generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(control_msgs REQUIRED) + find_package(controller_manager REQUIRED) + find_package(controller_interface REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ## create custom test function to pass yaml file into test main + #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + #add_executable(${TARGET} ${SOURCES}) + #_ament_cmake_gmock_find_gmock() + #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + #set(executable "$") + #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + #ament_add_test( + #${TARGET} + #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + #--gtest_output=xml:${result_file} + #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt + #RESULT_FILE ${result_file} + #) + #endfunction() + + # test loading admittance controller + add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) + target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + # test admittance controller function + add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_package() diff --git a/admittance_controller/admittance_controller.xml b/admittance_controller/admittance_controller.xml new file mode 100644 index 0000000000..3e4d5cb682 --- /dev/null +++ b/admittance_controller/admittance_controller.xml @@ -0,0 +1,9 @@ + + + + AdmittanceController ros2_control controller. + + + diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst new file mode 100644 index 0000000000..60837457f9 --- /dev/null +++ b/admittance_controller/doc/userdoc.rst @@ -0,0 +1,58 @@ +.. _joint_trajectory_controller_userdoc: + +Admittance Controller +====================== + +Admittance controller enables you do zero-force control from a force measured on your TCP. +The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. + +The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. + +Parameters: + + + +ROS2 interface of the controller +--------------------------------- + +Parameters +^^^^^^^^^^^ + +The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +An example parameter file can be found in the `test folder of the controller `_ + + +Topics +^^^^^^^ + +~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] + Target joint commands when controller is not in chained mode. + +~/state (output topic) [control_msgs::msg::AdmittanceControllerState] + Topic publishing internal states. + + +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +The controller has ``position`` and ``velocity`` reference interfaces exported in the format: +``//[position|velocity]`` + + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +If some interface is not provided, the last commanded interface will be used for calculation. + +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. + + +Commands +^^^^^^^^^ +The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp new file mode 100644 index 0000000000..f776646d42 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -0,0 +1,187 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +// include generated parameter library +#include "admittance_controller_parameters.hpp" + +#include "admittance_controller/admittance_rule.hpp" +#include "admittance_controller/visibility_control.h" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/force_torque_sensor.hpp" + +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace admittance_controller +{ +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +class AdmittanceController : public controller_interface::ChainableControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + size_t num_joints_ = 0; + std::vector command_joint_names_; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION}; + + // internal reference values + const std::vector allowed_reference_interfaces_types_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; + std::vector> position_reference_; + std::vector> velocity_reference_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + + // force torque sensor + std::unique_ptr force_torque_sensor_; + + // ROS subscribers + rclcpp::Subscription::SharedPtr + input_joint_command_subscriber_; + rclcpp::Publisher::SharedPtr s_publisher_; + + // admittance parameters + std::shared_ptr parameter_handler_; + + // ROS messages + std::shared_ptr joint_command_msg_; + + // real-time buffer + realtime_tools::RealtimeBuffer> + input_joint_command_; + std::unique_ptr> state_publisher_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; + trajectory_msgs::msg::JointTrajectoryPoint last_reference_; + + // control loop data + // reference_: reference value read by the controller + // joint_state_: current joint readings from the hardware + // reference_admittance_: reference value used by the controller after the admittance values are + // applied ft_values_: values read from the force torque sensor + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; + geometry_msgs::msg::Wrench ft_values_; + + /** + * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values + */ + void read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values); + + /** + * @brief Set fields of state_reference with values from controllers exported position and velocity references + */ + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + + /** +* @brief Write values from state_command to claimed hardware interfaces +*/ + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp new file mode 100644 index 0000000000..0f0aabb063 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,216 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "control_toolbox/filters.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "kinematics_interface/kinematics_interface.hpp" +#include "pluginlib/class_loader.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_kdl/tf2_kdl.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +struct AdmittanceTransforms +{ + // transformation from force torque sensor frame to base link frame at reference joint angles + Eigen::Isometry3d ref_base_ft_; + // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_ft_; + // transformation from control frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_control_; + // transformation from end effector frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_tip_; + // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_cog_; + // transformation from world frame to base link frame + Eigen::Isometry3d world_base_; +}; + +struct AdmittanceState +{ + explicit AdmittanceState(size_t num_joints) + { + admittance_velocity.setZero(); + admittance_acceleration.setZero(); + damping.setZero(); + mass.setOnes(); + mass_inv.setZero(); + stiffness.setZero(); + selected_axes.setZero(); + current_joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_vel = Eigen::VectorXd::Zero(num_joints); + joint_acc = Eigen::VectorXd::Zero(num_joints); + } + + Eigen::VectorXd current_joint_pos; + Eigen::VectorXd joint_pos; + Eigen::VectorXd joint_vel; + Eigen::VectorXd joint_acc; + Eigen::Matrix damping; + Eigen::Matrix mass; + Eigen::Matrix mass_inv; + Eigen::Matrix selected_axes; + Eigen::Matrix stiffness; + Eigen::Matrix wrench_base; + Eigen::Matrix admittance_acceleration; + Eigen::Matrix admittance_velocity; + Eigen::Isometry3d admittance_position; + Eigen::Matrix rot_base_control; + Eigen::Isometry3d ref_trans_base_ft; + std::string ft_sensor_frame; +}; + +class AdmittanceRule +{ +public: + explicit AdmittanceRule( + const std::shared_ptr & parameter_handler) + { + parameter_handler_ = parameter_handler; + parameters_ = parameter_handler_->get_params(); + num_joints_ = parameters_.joints.size(); + admittance_state_ = AdmittanceState(num_joints_); + reset(num_joints_); + } + + /// Configure admittance rule memory using number of joints. + controller_interface::return_type configure( + const std::shared_ptr & node, const size_t num_joint); + + /// Reset all values back to default + controller_interface::return_type reset(const size_t num_joints); + + /** + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does + * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation + * are calculated without an error + * \param[in] current_joint_state current joint state of the robot + * \param[in] reference_joint_state input joint state reference + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); + + /** + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field + * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + */ + void apply_parameters_update(); + + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. + * + * \param[in] current_joint_state current joint state of the robot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] reference_joint_state input joint state reference + * \param[in] period time in seconds since last controller update + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + */ + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + + /** + * Set fields of `state_message` from current admittance controller state. + * + * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + */ + const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + +public: + // admittance config parameters + std::shared_ptr parameter_handler_; + admittance_controller::Params parameters_; + +protected: + /** + * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input + * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin + * calls failed. + * \param[in] admittance_state contains all the information needed to calculate the admittance offset + * \param[in] dt controller period + * \param[out] success true if no calls to the kinematics interface fail + */ + bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); + + /** + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, + * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. + * The `wrench_world_` estimate includes gravity compensation + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame + * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + */ + void process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot); + + template + void vec_to_eigen(const std::vector & data, T2 & matrix); + + // number of robot joint + size_t num_joints_; + + // Kinematics interface plugin loader + std::shared_ptr> + kinematics_loader_; + std::unique_ptr kinematics_; + + // filtered wrench in world frame + Eigen::Matrix wrench_world_; + + // admittance controllers internal state + AdmittanceState admittance_state_{0}; + + // transforms needed for admittance update + AdmittanceTransforms admittance_transforms_; + + // position of center of gravity in cog_frame + Eigen::Vector3d cog_pos_; + + // force applied to sensor due to weight of end effector + Eigen::Vector3d end_effector_weight_; + + // ROS + control_msgs::msg::AdmittanceControllerState state_message_; +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp new file mode 100644 index 0000000000..87c16e4787 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -0,0 +1,405 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ + +#include "admittance_controller/admittance_rule.hpp" + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2_ros/transform_listener.h" + +namespace admittance_controller +{ +/// Configure admittance rule memory for num joints and load kinematics interface +controller_interface::return_type AdmittanceRule::configure( + const std::shared_ptr & node, const size_t num_joints) +{ + num_joints_ = num_joints; + + // initialize memory and values to zero (non-realtime function) + reset(num_joints); + + // Load the differential IK plugin + if (!parameters_.kinematics.plugin_name.empty()) + { + try + { + kinematics_loader_ = + std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); + kinematics_ = std::unique_ptr( + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( + node->get_node_parameters_interface(), parameters_.kinematics.tip)) + { + return controller_interface::return_type::ERROR; + } + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", + parameters_.kinematics.plugin_name.c_str(), ex.what()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), + "A differential IK plugin name was not specified in the config file."); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) +{ + // reset state message fields + state_message_.joint_state.name.assign(num_joints, ""); + state_message_.joint_state.position.assign(num_joints, 0); + state_message_.joint_state.velocity.assign(num_joints, 0); + state_message_.joint_state.effort.assign(num_joints, 0); + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name = parameters_.joints; + } + state_message_.mass.data.resize(6, 0.0); + state_message_.selected_axes.data.resize(6, 0); + state_message_.damping.data.resize(6, 0); + state_message_.stiffness.data.resize(6, 0); + state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.child_frame_id = "admittance_offset"; + + // reset admittance state + admittance_state_ = AdmittanceState(num_joints); + + // reset transforms and rotations + admittance_transforms_ = AdmittanceTransforms(); + + // reset forces + wrench_world_.setZero(); + end_effector_weight_.setZero(); + + // load/initialize Eigen types from parameters + apply_parameters_update(); + + return controller_interface::return_type::OK; +} + +void AdmittanceRule::apply_parameters_update() +{ + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + } + // update param values + end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); + + for (size_t i = 0; i < 6; ++i) + { + admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + } +} + +bool AdmittanceRule::get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) +{ + // get reference transforms + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + + // get transforms at current configuration + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.fixed_world_frame.frame.id, + admittance_transforms_.world_base_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.gravity_compensation.frame.id, + admittance_transforms_.base_cog_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.control.frame.id, + admittance_transforms_.base_control_); + + return success; +} + +// Update from reference joint states +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + const double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { + apply_parameters_update(); + } + + bool success = get_all_transforms(current_joint_state, reference_joint_state); + + // apply filter and update wrench_world_ vector + Eigen::Matrix rot_world_sensor = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); + Eigen::Matrix rot_world_cog = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); + + // transform wrench_world_ into base frame + admittance_state_.wrench_base.block<3, 1>(0, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_state_.wrench_base.block<3, 1>(3, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and don't + // modify the desired reference + if (!success) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (size_t i = 0; i < num_joints_; ++i) + { + desired_joint_state.positions[i] = + reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + desired_joint_state.velocities[i] = + reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + } + + return controller_interface::return_type::OK; +} + +bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) +{ + // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness + // correspond to the six diagonal elements of the stiffness matrix expressed in the control frame + auto rot_base_control = admittance_state.rot_base_control; + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix K_pos = Eigen::Matrix::Zero(); + Eigen::Matrix K_rot = Eigen::Matrix::Zero(); + K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + // Transform to the control frame + // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Force Control by Luigi Villani and Joris De Schutter + // Page 200 + K_pos = rot_base_control * K_pos * rot_base_control.transpose(); + K_rot = rot_base_control * K_rot * rot_base_control.transpose(); + K.block<3, 3>(0, 0) = K_pos; + K.block<3, 3>(3, 3) = K_rot; + + // The same for damping + Eigen::Matrix D = Eigen::Matrix::Zero(); + Eigen::Matrix D_pos = Eigen::Matrix::Zero(); + Eigen::Matrix D_rot = Eigen::Matrix::Zero(); + D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); + D_pos = rot_base_control * D_pos * rot_base_control.transpose(); + D_rot = rot_base_control * D_rot * rot_base_control.transpose(); + D.block<3, 3>(0, 0) = D_pos; + D.block<3, 3>(3, 3) = D_rot; + + // calculate admittance relative offset in base frame + Eigen::Isometry3d desired_trans_base_ft; + kinematics_->calculate_link_transform( + admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); + Eigen::Matrix X; + X.block<3, 1>(0, 0) = + desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); + auto R_ref = admittance_state.ref_trans_base_ft.rotation(); + auto R_desired = desired_trans_base_ft.rotation(); + auto R = R_desired * R_ref.transpose(); + auto angle_axis = Eigen::AngleAxisd(R); + X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); + + // get admittance relative velocity + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); + + // external force expressed in the base frame + auto F_base = admittance_state.wrench_base; + + // zero out any forces in the control frame + Eigen::Matrix F_control; + F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); + F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + F_control = F_control.cwiseProduct(admittance_state.selected_axes); + F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); + F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); + + // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x + Eigen::Matrix X_ddot = + admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( + admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, + admittance_state.joint_acc); + + // add damping if cartesian velocity falls below threshold + for (int64_t i = 0; i < admittance_state.joint_acc.size(); ++i) + { + admittance_state.joint_acc[i] -= + parameters_.admittance.joint_damping * admittance_state.joint_vel[i]; + } + + // integrate motion in joint space + admittance_state.joint_vel += (admittance_state.joint_acc) * dt; + admittance_state.joint_pos += admittance_state.joint_vel * dt; + + // calculate admittance velocity corresponding to joint velocity ("base_link" frame) + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_vel, + admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_acc, + admittance_state.ft_sensor_frame, admittance_state.admittance_acceleration); + + return success; +} + +void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot) +{ + Eigen::Matrix new_wrench; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; + + // transform to world frame + Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; + + // apply gravity compensation + new_wrench_base(2, 0) -= end_effector_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); + + // apply smoothing filter + for (size_t i = 0; i < 6; ++i) + { + wrench_world_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); + } +} + +const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() +{ + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name[i] = parameters_.joints[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + state_message_.damping.data[i] = admittance_state_.damping[i]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + + state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; + state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; + state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; + state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; + state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; + state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + + state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; + state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; + state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; + state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; + state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; + state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + + state_message_.admittance_acceleration.twist.linear.x = + admittance_state_.admittance_acceleration[0]; + state_message_.admittance_acceleration.twist.linear.y = + admittance_state_.admittance_acceleration[1]; + state_message_.admittance_acceleration.twist.linear.z = + admittance_state_.admittance_acceleration[2]; + state_message_.admittance_acceleration.twist.angular.x = + admittance_state_.admittance_acceleration[3]; + state_message_.admittance_acceleration.twist.angular.y = + admittance_state_.admittance_acceleration[4]; + state_message_.admittance_acceleration.twist.angular.z = + admittance_state_.admittance_acceleration[5]; + + state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; + state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + + Eigen::Quaterniond quat(admittance_state_.rot_base_control); + state_message_.rot_base_control.w = quat.w(); + state_message_.rot_base_control.x = quat.x(); + state_message_.rot_base_control.y = quat.y(); + state_message_.rot_base_control.z = quat.z(); + + state_message_.ft_sensor_frame.data = + admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here + + return state_message_; +} + +template +void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) +{ + for (auto col = 0; col < matrix.cols(); col++) + { + for (auto row = 0; row < matrix.rows(); row++) + { + matrix(row, col) = data[row + col * matrix.rows()]; + } + } +} + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/include/admittance_controller/visibility_control.h b/admittance_controller/include/admittance_controller/visibility_control.h new file mode 100644 index 0000000000..24f17a5c2c --- /dev/null +++ b/admittance_controller/include/admittance_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define ADMITTANCE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define ADMITTANCE_CONTROLLER_EXPORT __declspec(dllexport) +#define ADMITTANCE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef ADMITTANCE_CONTROLLER_BUILDING_DLL +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_EXPORT +#else +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_IMPORT +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#else +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define ADMITTANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml new file mode 100644 index 0000000000..e352dcfa91 --- /dev/null +++ b/admittance_controller/package.xml @@ -0,0 +1,43 @@ + + + + admittance_controller + 0.0.0 + Implementation of admittance controllers for different input and output interface. + Denis Štogl + Andy Zelenak + Apache License 2.0 + + ament_cmake + + control_msgs + control_toolbox + controller_interface + kinematics_interface + filters + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs + + + ament_cmake_gmock + control_msgs + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp new file mode 100644 index 0000000000..1af4e1317f --- /dev/null +++ b/admittance_controller/src/admittance_controller.cpp @@ -0,0 +1,592 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#include "admittance_controller/admittance_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "admittance_controller/admittance_rule_impl.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "rcutils/logging_macros.h" +#include "tf2_ros/buffer.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +controller_interface::CallbackReturn AdmittanceController::on_init() +{ + // initialize controller config + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // number of joints in controllers is fixed after initialization + num_joints_ = admittance_->parameters_.joints.size(); + + // allocate dynamic memory + last_reference_.positions.assign(num_joints_, 0.0); + last_reference_.velocities.assign(num_joints_, 0.0); + last_reference_.accelerations.assign(num_joints_, 0.0); + last_commanded_ = last_reference_; + reference_ = last_reference_; + reference_admittance_ = last_reference_; + joint_state_ = last_reference_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() + const +{ + std::vector command_interfaces_config_names; + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + for (const auto & joint : command_joint_names_) + { + auto full_name = joint + "/" + interface; + command_interfaces_config_names.push_back(full_name); + } + } + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, + command_interfaces_config_names}; +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() + const +{ + std::vector state_interfaces_config_names; + for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) + { + const auto & interface = admittance_->parameters_.state_interfaces[i]; + for (const auto & joint : admittance_->parameters_.joints) + { + auto full_name = joint + "/" + interface; + state_interfaces_config_names.push_back(full_name); + } + } + + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config_names.insert( + state_interfaces_config_names.end(), ft_interfaces.begin(), ft_interfaces.end()); + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; +} + +std::vector +AdmittanceController::on_export_reference_interfaces() +{ + // create CommandInterface interfaces that other controllers will be able to chain with + if (!admittance_) + { + return {}; + } + + std::vector chainable_command_interfaces; + const auto num_chainable_interfaces = + admittance_->parameters_.chainable_command_interfaces.size() * + admittance_->parameters_.joints.size(); + + // allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + position_reference_ = {}; + velocity_reference_ = {}; + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : allowed_reference_interfaces_types_) + { + for (const auto & joint : admittance_->parameters_.joints) + { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); + } + const auto full_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + + index++; + } + } + + return chainable_command_interfaces; +} + +controller_interface::CallbackReturn AdmittanceController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) + { + command_joint_names_ = admittance_->parameters_.joints; + RCLCPP_INFO( + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != num_joints_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // print and validate interface types + for (const auto & tmp : admittance_->parameters_.state_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto & tmp : admittance_->parameters_.command_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + + // validate exported interfaces + for (const auto & tmp : admittance_->parameters_.chainable_command_interfaces) + { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + auto contains_interface_type = + [](const std::vector & interface_type_list, const std::string & interface_type) + { + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); + }; + + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT); + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + + auto get_interface_list = [](const std::vector & interface_types) + { + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_command_interfaces << " "; + } + ss_command_interfaces << interface_types[index]; + } + return ss_command_interfaces.str(); + }; + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(admittance_->parameters_.command_interfaces).c_str(), + get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + + // setup subscribers and publishers + auto joint_command_callback = + [this](const std::shared_ptr msg) + { input_joint_command_.writeFromNonRT(msg); }; + input_joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + s_publisher_ = get_node()->create_publisher( + "~/status", rclcpp::SystemDefaultsQoS()); + state_publisher_ = + std::make_unique>(s_publisher_); + + // Initialize state message + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlock(); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + + // configure admittance rule + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // on_activate is called when the lifecycle node activates. + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // order all joints in the storage + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, + interface.c_str(), joint_command_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + + // update parameters if any have changed + admittance_->apply_parameters_update(); + + // initialize interface of the FTS semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + + // initialize states + read_state_from_hardware(joint_state_, ft_values_); + for (auto val : joint_state_.positions) + { + if (std::isnan(val)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Use current joint_state as a default reference + last_reference_ = joint_state_; + last_commanded_ = joint_state_; + reference_ = joint_state_; + reference_admittance_ = joint_state_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +{ + // update input reference from ros subscriber message + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + joint_command_msg_ = *input_joint_command_.readFromRT(); + + // if message exists, load values into references + if (joint_command_msg_.get()) + { + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + { + position_reference_[i].get() = joint_command_msg_->positions[i]; + } + for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) + { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // Realtime constraints are required in this function + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + // update input reference from chainable interfaces + read_state_reference_interfaces(reference_); + + // get all controller inputs + read_state_from_hardware(joint_state_, ft_values_); + + // apply admittance control to reference to determine desired state + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + // write calculated values to joint interfaces + write_state_to_hardware(reference_admittance_); + + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlockAndPublish(); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn AdmittanceController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // release force torque sensor interface + force_torque_sensor_->release_interfaces(); + + // reset to prevent stale references + for (size_t i = 0; i < num_joints_; i++) + { + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } + + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + admittance_->reset(num_joints_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_error( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + admittance_->reset(num_joints_); + return controller_interface::CallbackReturn::SUCCESS; +} + +void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values) +{ + // if any interface has nan values, assume state_current is the last command state + bool nan_position = false; + bool nan_velocity = false; + bool nan_acceleration = false; + + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_state_interface_) + { + state_current.positions[joint_ind] = + state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); + nan_position |= std::isnan(state_current.positions[joint_ind]); + } + else if (has_velocity_state_interface_) + { + state_current.velocities[joint_ind] = + state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); + nan_velocity |= std::isnan(state_current.velocities[joint_ind]); + } + else if (has_acceleration_state_interface_) + { + state_current.accelerations[joint_ind] = + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); + nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); + } + } + + if (nan_position) + { + state_current.positions = last_commanded_.positions; + } + if (nan_velocity) + { + state_current.velocities = last_commanded_.velocities; + } + if (nan_acceleration) + { + state_current.accelerations = last_commanded_.accelerations; + } + + // if any ft_values are nan, assume values are zero + force_torque_sensor_->get_values_as_message(ft_values); + if ( + std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || + std::isnan(ft_values.force.z) || std::isnan(ft_values.torque.x) || + std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)) + { + ft_values = geometry_msgs::msg::Wrench(); + } +} + +void AdmittanceController::write_state_to_hardware( + const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) +{ + // if any interface has nan values, assume state_commanded is the last command state + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_command_interface_) + { + command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_velocity_command_interface_) + { + command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_acceleration_command_interface_) + { + command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + } + last_commanded_ = state_commanded; +} + +void AdmittanceController::read_state_reference_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint & state_reference) +{ + // TODO(destogl): check why is this here? + + // if any interface has nan values, assume state_reference is the last set reference + for (size_t i = 0; i < num_joints_; ++i) + { + // update position + if (std::isnan(position_reference_[i])) + { + position_reference_[i].get() = last_reference_.positions[i]; + } + state_reference.positions[i] = position_reference_[i]; + + // update velocity + if (std::isnan(velocity_reference_[i])) + { + velocity_reference_[i].get() = last_reference_.velocities[i]; + } + state_reference.velocities[i] = velocity_reference_[i]; + } + + last_reference_.positions = state_reference.positions; + last_reference_.velocities = state_reference.velocities; +} + +} // namespace admittance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + admittance_controller::AdmittanceController, controller_interface::ChainableControllerInterface) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml new file mode 100644 index 0000000000..5bdc40e398 --- /dev/null +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -0,0 +1,159 @@ +admittance_controller: + joints: { + type: string_array, + description: "Specifies which joints will be used by the controller. ", + read_only: true + } + command_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies the joints for writing into another controllers reference. This parameter is only relevant when chaining the output of this controller to the input of another controller.", + read_only: true + } + command_interfaces: + { + type: string_array, + description: "Specifies which command interfaces the controller will claim.", + read_only: true + } + + state_interfaces: + { + type: string_array, + description: "Specifies which state interfaces the controller will claim.", + read_only: true + } + + chainable_command_interfaces: + { + type: string_array, + description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", + read_only: true + } + + kinematics: + plugin_name: { + type: string, + description: "Specifies the name of the kinematics plugin to load." + } + plugin_package: { + type: string, + description: "Specifies the package name that contains the kinematics plugin." + } + base: { + type: string, + description: "Specifies the base link of the robot description used by the kinematics plugin." + } + tip: { + type: string, + description: "Specifies the end effector link of the robot description used by the kinematics plugin." + } + alpha: { + type: double, + default_value: 0.01, + description: "Specifies the damping coefficient for the Jacobian pseudo inverse." + } + + ft_sensor: + name: { + type: string, + description: "Specifies the name of the force torque sensor in the robot description which will be used in the admittance calculation." + } + frame: + id: { + type: string, + description: "Specifies the frame/link name of the force torque sensor." + } + filter_coefficient: { + type: double, + default_value: 0.05, + description: "Specifies the filter coefficient for the sensor's exponential filter." + } + + control: + frame: + id: { + type: string, + description: "Specifies the control frame used for admittance calculation." + } + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: { + type: string, + description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." + } + + gravity_compensation: + frame: + id: { + type: string, + description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." + } + CoG: + pos: { + type: double_array, + description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", + validation: { + fixed_size<>: 3 + } + } + force: { + type: double, + default_value: 0.0, + description: "Specifies the weight of the end effector, e.g mass * 9.81." + } + + admittance: + selected_axes: + { + type: bool_array, + description: "Specifies whether the axes x, y, z, rx, ry, and rz should be included in the admittance calculation.", + validation: { + fixed_size<>: 6 + } + } + mass: { + type: double_array, + description: "Specifies the mass values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0001, 1000000.0 ] + } + } + damping_ratio: { + type: double_array, + description: "Specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + The damping ratio is defined as: zeta = D / (2 * sqrt( M * S )).", + validation: { + fixed_size<>: 6 + } + } + stiffness: { + type: double_array, + description: "Specifies the stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0, 100000000.0 ] + } + } + joint_damping: { + type: double, + description: "Specifies the joint damping applied used in the admittance calculation.", + default_value: 5, + validation: { + lower_bounds: [ 0.0 ] + } + } + + # general settings + robot_description: { + type: string, + description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", + read_only: true + } + enable_parameter_update_without_reactivation: { + type: bool, + default_value: true, + description: "If enabled, the parameters will be dynamically updated while the controller is running." + } diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/6d_robot_description.hpp new file mode 100644 index 0000000000..f67b5bd054 --- /dev/null +++ b/admittance_controller/test/6d_robot_description.hpp @@ -0,0 +1,313 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ + +#include + +namespace ros2_control_test_assets +{ +const auto valid_6d_robot_urdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ros2_control_kuka_demo_driver/KukaSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + +)"; + +const auto valid_6d_robot_srdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace ros2_control_test_assets + +#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..d056e0406c --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,292 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#include "test_admittance_controller.hpp" + +#include +#include +#include +#include + +// Test on_configure returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +{ + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingParameters, + ::testing::Values( + "admittance.mass", "admittance.selected_axes", "admittance.stiffness", + "chainable_command_interfaces", "command_interfaces", "control.frame.id", + "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, + ::testing::Values( + // wrong length COG + std::make_tuple( + std::string("gravity_compensation.CoG.pos"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), + // wrong length stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length mass + std::make_tuple( + std::string("admittance.mass"), rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative mass + std::make_tuple( + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length damping ratio + std::make_tuple( + std::string("admittance.damping_ratio"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // wrong length selected axes + std::make_tuple( + std::string("admittance.selected_axes"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // invalid robot description + std::make_tuple( + std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + +TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) +{ + auto result = SetUpController(); + + ASSERT_EQ(result, controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.joints.begin(), + controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.command_interfaces.size() == + command_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.command_interfaces.begin(), + controller_->admittance_->parameters_.command_interfaces.end(), + command_interface_types_.begin(), command_interface_types_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.state_interfaces.begin(), + controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), + state_interface_types_.end())); + + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.selected_axes.size() == + admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.selected_axes.begin(), + controller_->admittance_->parameters_.admittance.selected_axes.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.mass.begin(), + controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), + admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.damping_ratio.size() == + admittance_damping_ratio_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.stiffness.size() == + admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.stiffness.begin(), + controller_->admittance_->parameters_.admittance.stiffness.end(), admittance_stiffness_.begin(), + admittance_stiffness_.end())); +} + +TEST_F(AdmittanceControllerTest, check_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + + ASSERT_EQ( + controller_->state_interfaces_.size(), + state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); +} + +TEST_F(AdmittanceControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + assign_interfaces(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // // Check that wrench command are all zero since not used + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + + // // Check joint command message + // for (auto i = 0ul; i < joint_names_.size(); i++) + // { + // ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); + // ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); + // } +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // After first update state, commanded position should be near the start state + for (auto i = 0ul; i < joint_state_values_.size(); i++) + { + ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} + +// Add test, wrong interfaces diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp new file mode 100644 index 0000000000..be78f05bb9 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,463 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Denis Stogl + +#ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ +#define TEST_ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "6d_robot_description.hpp" +#include "admittance_controller/admittance_controller.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "semantic_components/force_torque_sensor.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ +const double COMMON_THRESHOLD = 0.001; + +constexpr auto NODE_SUCCESS = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} + +} // namespace + +// subclassing and friending so we can access member variables +class TestableAdmittanceController : public admittance_controller::AdmittanceController +{ + FRIEND_TEST(AdmittanceControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, interface_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AdmittanceControllerTest, check_interfaces); + FRIEND_TEST(AdmittanceControllerTest, activate_success); + FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + +public: + CallbackReturn on_init() override + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + return admittance_controller::AdmittanceController::on_init(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = + input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + + if (success) + { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; +}; + +class AdmittanceControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + // rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + force_command_publisher_ = + command_publisher_node_->create_publisher( + "/test_admittance_controller/force_references", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ =command_publisher_node_->create_publisher( + // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + joint_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/joint_references", rclcpp::SystemDefaultsQoS()); + + test_subscription_node_ = std::make_shared("test_subscription_node"); + test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); + } + + static void TearDownTestCase() + { + // rclcpp::shutdown(); + } + + void TearDown() { controller_.reset(nullptr); } + +protected: + controller_interface::return_type SetUpController( + const std::string & controller_name, const std::vector & parameter_overrides) + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpController( + const std::string & controller_name = "test_admittance_controller") + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpControllerCommon( + const std::string & controller_name, const rclcpp::NodeOptions & options) + { + auto result = controller_->init(controller_name, "", options); + + controller_->export_reference_interfaces(); + assign_interfaces(); + + return result; + } + + void assign_interfaces() + { + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); + fts_state_names_ = sc_fts.get_state_interface_names(); + std::vector state_ifs; + + const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + state_itfs_.reserve(num_state_ifs); + state_ifs.reserve(num_state_ifs); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector fts_itf_names = {"force.x", "force.y", "force.z", + "torque.x", "torque.y", "torque.z"}; + + for (auto i = 0u; i < fts_state_names_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void broadcast_tfs() + { + static tf2_ros::TransformBroadcaster br(test_broadcaster_node_); + geometry_msgs::msg::TransformStamped transform_stamped; + + transform_stamped.header.stamp = test_broadcaster_node_->now(); + transform_stamped.header.frame_id = fixed_world_frame_; + transform_stamped.transform.translation.x = 1.3; + transform_stamped.transform.translation.y = 0.5; + transform_stamped.transform.translation.z = 0.5; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_tip_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.header.frame_id = ik_tip_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 0; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = control_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.05; + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.2; + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node_->create_subscription( + "/test_admittance_controller/status", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // wait_for_topic(force_command_publisher_->get_topic_name()); + // } + + // wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandWrenchMsg force_msg; + force_msg.header.frame_id = sensor_frame_; + force_msg.wrench.force.x = 0.1; + force_msg.wrench.force.y = 0.2; + force_msg.wrench.force.z = 0.3; + force_msg.wrench.torque.x = 3.1; + force_msg.wrench.torque.y = 3.2; + force_msg.wrench.torque.z = 3.3; + + ControllerCommandPoseMsg pose_msg; + pose_msg.header.frame_id = endeffector_frame_; + pose_msg.pose.position.x = 0.1; + pose_msg.pose.position.y = 0.2; + pose_msg.pose.position.z = 0.3; + pose_msg.pose.orientation.x = 0; + pose_msg.pose.orientation.y = 0; + pose_msg.pose.orientation.z = 0; + pose_msg.pose.orientation.w = 1; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // force_command_publisher_->publish(force_msg); + // } + // pose_command_publisher_->publish(pose_msg); + + wait_for_topic(joint_command_publisher_->get_topic_name()); + + ControllerCommandJointMsg joint_msg; + // joint_msg.joint_names = joint_names_; + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + auto num_joints = joint_names_.size(); + trajectory_point.positions.reserve(num_joints); + trajectory_point.velocities.resize(num_joints, 0.0); + for (auto index = 0u; index < num_joints; ++index) + { + trajectory_point.positions.emplace_back(joint_state_values_[index]); + } + joint_msg = trajectory_point; + + joint_command_publisher_->publish(joint_msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = {"joint1", "joint2", "joint3", + "joint4", "joint5", "joint6"}; + const std::vector command_interface_types_ = {"position"}; + const std::vector state_interface_types_ = {"position"}; + const std::string ft_sensor_name_ = "ft_sensor_name"; + + bool hardware_state_has_offset_ = false; + + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "arm"; + // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + + const std::string control_frame_ = "tool0"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "link_6"; + + std::array admittance_selected_axes_ = {true, true, true, true, true, true}; + std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427, + 2.828427, 2.828427, 2.828427}; + std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; + + std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector fts_state_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr force_command_publisher_; + // rclcpp::Publisher::SharedPtr pose_command_publisher_; + rclcpp::Publisher::SharedPtr joint_command_publisher_; + rclcpp::Node::SharedPtr test_subscription_node_; + rclcpp::Node::SharedPtr test_broadcaster_node_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedMissingParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); + auto node = std::make_shared("test_admittance_controller"); + overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); + } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + controller_interface::return_type SetUpController(const std::string & remove_name) + { + std::vector parameter_overrides; + for (const auto & override : overrides_) + { + if (override.first != remove_name) + { + rclcpp::Parameter param(override.first, override.second); + parameter_overrides.push_back(param); + } + } + + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller_no_overrides", parameter_overrides); + } + + std::map overrides_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedInvalidParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { AdmittanceControllerTest::SetUp(); } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + void SetUpController() + { + AdmittanceControllerTest::SetUpController("test_admittance_controller"); + } +}; + +#endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp new file mode 100644 index 0000000000..1f290aeff6 --- /dev/null +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAdmittanceController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml new file mode 100644 index 0000000000..22cbda2df9 --- /dev/null +++ b/admittance_controller/test/test_params.yaml @@ -0,0 +1,111 @@ +load_admittance_controller: + # contains minimal parameters that need to be set to load controller + ros__parameters: + joints: + - joint1 + - joint2 + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + chainable_command_interfaces: + - position + - velocity + + +test_admittance_controller: + # contains minimal needed parameters for kuka_kr6 + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: tool0 + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 09ec6e5387..d9a1c841a5 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 2011f0c31a..b9b8674fc9 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 3a667e7d9a..8c0ba1ff8a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -3,11 +3,19 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 6f9dc84a3f..f62ba87bf3 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -3,15 +3,20 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master - generate_parameter_library: + control_msgs: type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master From eecb7f7af8cd750b9b4116947ae31fb5df52dcfd Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 11 Oct 2022 15:42:50 -0600 Subject: [PATCH 159/524] Fix parameter library export (#448) --- admittance_controller/CMakeLists.txt | 39 +++++-------------- .../CMakeLists.txt | 13 +++---- force_torque_sensor_broadcaster/package.xml | 3 +- joint_trajectory_controller/CMakeLists.txt | 17 ++++---- joint_trajectory_controller/package.xml | 2 +- 5 files changed, 24 insertions(+), 50 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index bbfe19deda..a6dcf35dce 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -38,15 +38,15 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) -target_include_directories(${PROJECT_NAME} PRIVATE include) -generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) -target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(admittance_controller SHARED src/admittance_controller.cpp) +target_include_directories(admittance_controller PRIVATE include) +generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(admittance_controller admittance_controller_parameters) +ament_target_dependencies(admittance_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) @@ -54,7 +54,8 @@ install(DIRECTORY include/ DESTINATION include ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS admittance_controller admittance_controller_parameters + EXPORT export_admittance_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -68,23 +69,6 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ## create custom test function to pass yaml file into test main - #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) - #add_executable(${TARGET} ${SOURCES}) - #_ament_cmake_gmock_find_gmock() - #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") - #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) - #set(executable "$") - #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") - #ament_add_test( - #${TARGET} - #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} - #--gtest_output=xml:${result_file} - #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt - #RESULT_FILE ${result_file} - #) - #endfunction() - # test loading admittance controller add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) @@ -110,13 +94,10 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include +ament_export_targets( + export_admittance_controller HAS_LIBRARY_TARGET ) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index cdb1fe2b35..3aa57d4aad 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -13,6 +13,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + generate_parameter_library geometry_msgs hardware_interface pluginlib @@ -26,7 +27,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) @@ -54,8 +54,8 @@ pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) install( - TARGETS - ${PROJECT_NAME} + TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters + EXPORT export_force_torque_sensor_broadcaster RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -101,11 +101,8 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +ament_export_targets( + export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET ) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index bf1e01bde7..0e105ceb2c 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,8 +11,6 @@ ament_cmake - generate_parameter_library - controller_interface geometry_msgs hardware_interface @@ -20,6 +18,7 @@ rclcpp rclcpp_lifecycle realtime_tools + generate_parameter_library ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 6d443c532f..32461617ac 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -13,9 +13,10 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS angles - controller_interface control_msgs control_toolbox + controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp @@ -29,8 +30,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) - generate_parameter_library(joint_trajectory_controller_parameters src/joint_trajectory_controller_parameters.yaml include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -52,7 +51,8 @@ install(DIRECTORY include/ DESTINATION include ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -105,13 +105,10 @@ if(BUILD_TESTING) # ) endif() +ament_export_targets( + export_joint_trajectory_controller HAS_LIBRARY_TARGET +) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b3a828efe2..f768c82a8f 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -12,7 +12,6 @@ ament_cmake angles - generate_parameter_library pluginlib realtime_tools @@ -27,6 +26,7 @@ rclcpp rclcpp_lifecycle trajectory_msgs + generate_parameter_library ament_cmake_gmock controller_manager From eccea9cef5f5dbebe1f5fda68d1af0e371d000d9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 13 Oct 2022 10:24:10 -0600 Subject: [PATCH 160/524] Generate params for ForwardCommandController (#396) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Generate params for ForwardCommandController Signed-off-by: Tyler Weaver * Clean up CMake Signed-off-by: Tyler Weaver Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- forward_command_controller/CMakeLists.txt | 39 ++++++++++++------- .../forward_command_controller.hpp | 6 ++- ...i_interface_forward_command_controller.hpp | 9 ++++- forward_command_controller/package.xml | 1 + .../src/forward_command_controller.cpp | 22 +++++------ ...forward_command_controller_parameters.yaml | 11 ++++++ ...i_interface_forward_command_controller.cpp | 20 ++++++---- ...forward_command_controller_parameters.yaml | 11 ++++++ ros2_controllers-not-released.rolling.repos | 11 ++---- ros2_controllers.rolling.repos | 8 ++++ 10 files changed, 93 insertions(+), 45 deletions(-) create mode 100644 forward_command_controller/src/forward_command_controller_parameters.yaml create mode 100644 forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index f1d7a18328..ef8581d195 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -13,6 +13,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp @@ -26,17 +27,30 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} +generate_parameter_library( + forward_command_controller_parameters + src/forward_command_controller_parameters.yaml +) +generate_parameter_library( + multi_interface_forward_command_controller_parameters + src/multi_interface_forward_command_controller_parameters.yaml +) + +add_library(forward_command_controller SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(forward_command_controller PRIVATE include) +ament_target_dependencies(forward_command_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") +target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) install( @@ -46,10 +60,14 @@ install( install( TARGETS - ${PROJECT_NAME} + forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + EXPORT export_forward_command_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) if(BUILD_TESTING) @@ -75,7 +93,7 @@ if(BUILD_TESTING) ) target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller - ${PROJECT_NAME} + forward_command_controller ) ament_add_gmock( @@ -96,15 +114,10 @@ if(BUILD_TESTING) ) target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller - ${PROJECT_NAME} + forward_command_controller ) endif() +ament_export_targets(export_forward_command_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index f50af328d8..8a697847cd 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -15,11 +15,13 @@ #ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ +#include #include #include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "forward_command_controller_parameters.hpp" namespace forward_command_controller { @@ -44,8 +46,8 @@ class ForwardCommandController : public ForwardControllersBase void declare_parameters() override; controller_interface::CallbackReturn read_parameters() override; - std::vector joint_names_; - std::string interface_name_; + std::shared_ptr param_listener_; + Params params_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 8acec36c5c..fd7c0d480e 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -15,11 +15,13 @@ #ifndef FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ +#include #include #include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "multi_interface_forward_command_controller_parameters.hpp" namespace forward_command_controller { @@ -45,8 +47,11 @@ class MultiInterfaceForwardCommandController void declare_parameters() override; controller_interface::CallbackReturn read_parameters() override; - std::string joint_name_; - std::vector interface_names_; + using Params = multi_interface_forward_command_controller::Params; + using ParamListener = multi_interface_forward_command_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f4c137301a..47e12892ca 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 06c6b15120..0305a37e4e 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -29,35 +29,33 @@ ForwardCommandController::ForwardCommandController() : ForwardControllersBase() void ForwardCommandController::declare_parameters() { - auto_declare("joints", std::vector()); - auto_declare("interface_name", std::string()); + param_listener_ = std::make_shared(get_node()); } controller_interface::CallbackReturn ForwardCommandController::read_parameters() { - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) + if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } + params_ = param_listener_->get_params(); - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) + if (params_.joints.empty()) { - interface_name_ = get_node()->get_parameter("interface_name").as_string(); + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; } - if (interface_name_.empty()) + if (params_.interface_name.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); return controller_interface::CallbackReturn::ERROR; } - for (const auto & joint : joint_names_) + for (const auto & joint : params_.joints) { - command_interface_types_.push_back(joint + "/" + interface_name_); + command_interface_types_.push_back(joint + "/" + params_.interface_name); } return controller_interface::CallbackReturn::SUCCESS; diff --git a/forward_command_controller/src/forward_command_controller_parameters.yaml b/forward_command_controller/src/forward_command_controller_parameters.yaml new file mode 100644 index 0000000000..c633fab268 --- /dev/null +++ b/forward_command_controller/src/forward_command_controller_parameters.yaml @@ -0,0 +1,11 @@ +forward_command_controller: + joints: { + type: string_array, + default_value: [], + description: "Name of the joints to control", + } + interface_name: { + type: string, + default_value: "", + description: "Name of the interface to command", + } diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 8faacd0776..06fe395cb4 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,6 +14,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" +#include #include #include @@ -26,30 +27,33 @@ MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() void MultiInterfaceForwardCommandController::declare_parameters() { - auto_declare("joint", joint_name_); - auto_declare("interface_names", interface_names_); + param_listener_ = std::make_shared(get_node()); } controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() { - joint_name_ = get_node()->get_parameter("joint").as_string(); - interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); - if (joint_name_.empty()) + if (params_.joint.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); return controller_interface::CallbackReturn::ERROR; } - if (interface_names_.empty()) + if (params_.interface_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); return controller_interface::CallbackReturn::ERROR; } - for (const auto & interface : interface_names_) + for (const auto & interface : params_.interface_names) { - command_interface_types_.push_back(joint_name_ + "/" + interface); + command_interface_types_.push_back(params_.joint + "/" + interface); } return controller_interface::CallbackReturn::SUCCESS; diff --git a/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml b/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml new file mode 100644 index 0000000000..d1eb1cacc9 --- /dev/null +++ b/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml @@ -0,0 +1,11 @@ +multi_interface_forward_command_controller: + joint: { + type: string, + default_value: "", + description: "Name of the joint to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index b9cdead153..66352f4960 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1,10 +1,5 @@ repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master - control_msgs: + generate_parameter_library: type: git - url: https://github.com/ros-controls/control_msgs.git - version: humble + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index f62ba87bf3..dfed17059b 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -20,3 +20,11 @@ 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 From 0cfa8046a808bd16b54b171f0f694209eb919544 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 20 Oct 2022 23:22:29 -0600 Subject: [PATCH 161/524] Generate parameters for IMU Sensor Broadcaster (#399) --- imu_sensor_broadcaster/CMakeLists.txt | 61 ++++++++++--------- .../imu_sensor_broadcaster.hpp | 5 +- imu_sensor_broadcaster/package.xml | 1 + .../src/imu_sensor_broadcaster.cpp | 18 +++--- .../imu_sensor_broadcaster_parameters.yaml | 11 ++++ 5 files changed, 55 insertions(+), 41 deletions(-) create mode 100644 imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 5febe1c048..e49fb65ad3 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -13,12 +13,13 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface - sensor_msgs + generate_parameter_library hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools + sensor_msgs ) find_package(ament_cmake REQUIRED) @@ -26,37 +27,29 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(imu_sensor_broadcaster_parameters + src/imu_sensor_broadcaster_parameters.yaml +) + add_library( - ${PROJECT_NAME} - SHARED + imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_include_directories( - ${PROJECT_NAME} - PUBLIC - include +target_include_directories(imu_sensor_broadcaster + PRIVATE $ + PRIVATE $ ) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(imu_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") +target_link_libraries(imu_sensor_broadcaster + imu_sensor_broadcaster_parameters +) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -80,7 +73,7 @@ if(BUILD_TESTING) ) target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster - ${PROJECT_NAME} + imu_sensor_broadcaster ) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface @@ -92,14 +85,22 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +install( + TARGETS + imu_sensor_broadcaster + imu_sensor_broadcaster_parameters + EXPORT export_imu_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) +ament_export_targets(export_imu_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 11310b5b70..d78e28780e 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" +#include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" @@ -61,8 +62,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::string sensor_name_; - std::string frame_id_; + std::shared_ptr param_listener_; + Params params_; std::unique_ptr imu_sensor_; diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index bc9d930830..855d3fe58e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -12,6 +12,7 @@ ament_cmake controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index b609bca8c7..e488d13638 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -27,8 +27,8 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() { try { - auto_declare("sensor_name", ""); - auto_declare("frame_id", ""); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -43,22 +43,22 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); - if (sensor_name_.empty()) + + params_ = param_listener_->get_params(); + if (params_.sensor_name.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = get_node()->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) + if (params_.frame_id.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } - imu_sensor_ = - std::make_unique(semantic_components::IMUSensor(sensor_name_)); + imu_sensor_ = std::make_unique( + semantic_components::IMUSensor(params_.sensor_name)); try { // register ft sensor data publisher @@ -75,7 +75,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( } realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = frame_id_; + realtime_publisher_->msg_.header.frame_id = params_.frame_id; realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..613d42c02d --- /dev/null +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -0,0 +1,11 @@ +imu_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } From 3f9f55038c0125dcc4a1fc831fc34e2fc20e383f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 21 Oct 2022 07:44:44 +0200 Subject: [PATCH 162/524] [IMU Broadcaster] Added parameters for definition of static covariances. (#455) --- .../src/imu_sensor_broadcaster.cpp | 10 ++++++- .../imu_sensor_broadcaster_parameters.yaml | 24 +++++++++++++++ .../test/test_imu_sensor_broadcaster.cpp | 29 ++++++++++++------- 3 files changed, 51 insertions(+), 12 deletions(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index e488d13638..3e2d574f0f 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -43,7 +43,6 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - params_ = param_listener_->get_params(); if (params_.sensor_name.empty()) { @@ -76,6 +75,15 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; + // convert double vector to fixed-size array in the message + for (size_t i = 0; i < 9; ++i) + { + realtime_publisher_->msg_.orientation_covariance[i] = params_.static_covariance_orientation[i]; + realtime_publisher_->msg_.angular_velocity_covariance[i] = + params_.static_covariance_angular_velocity[i]; + realtime_publisher_->msg_.linear_acceleration_covariance[i] = + params_.static_covariance_linear_acceleration[i]; + } realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index 613d42c02d..c0daba9f11 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -9,3 +9,27 @@ imu_sensor_broadcaster: default_value: "", description: "Sensor's frame_id in which values are published.", } + static_covariance_orientation: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static orientation covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } + static_covariance_angular_velocity: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static angular velocity covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } + static_covariance_linear_acceleration: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static linear acceleration covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index cc1398a9b8..31c8d9e2f0 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -189,15 +189,22 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) sensor_msgs::msg::Imu imu_msg; subscribe_and_get_message(imu_msg); - ASSERT_EQ(imu_msg.header.frame_id, frame_id_); - ASSERT_EQ(imu_msg.orientation.x, sensor_values_[0]); - ASSERT_EQ(imu_msg.orientation.y, sensor_values_[1]); - ASSERT_EQ(imu_msg.orientation.z, sensor_values_[2]); - ASSERT_EQ(imu_msg.orientation.w, sensor_values_[3]); - ASSERT_EQ(imu_msg.angular_velocity.x, sensor_values_[4]); - ASSERT_EQ(imu_msg.angular_velocity.y, sensor_values_[5]); - ASSERT_EQ(imu_msg.angular_velocity.z, sensor_values_[6]); - ASSERT_EQ(imu_msg.linear_acceleration.x, sensor_values_[7]); - ASSERT_EQ(imu_msg.linear_acceleration.y, sensor_values_[8]); - ASSERT_EQ(imu_msg.linear_acceleration.z, sensor_values_[9]); + EXPECT_EQ(imu_msg.header.frame_id, frame_id_); + EXPECT_EQ(imu_msg.orientation.x, sensor_values_[0]); + EXPECT_EQ(imu_msg.orientation.y, sensor_values_[1]); + EXPECT_EQ(imu_msg.orientation.z, sensor_values_[2]); + EXPECT_EQ(imu_msg.orientation.w, sensor_values_[3]); + EXPECT_EQ(imu_msg.angular_velocity.x, sensor_values_[4]); + EXPECT_EQ(imu_msg.angular_velocity.y, sensor_values_[5]); + EXPECT_EQ(imu_msg.angular_velocity.z, sensor_values_[6]); + EXPECT_EQ(imu_msg.linear_acceleration.x, sensor_values_[7]); + EXPECT_EQ(imu_msg.linear_acceleration.y, sensor_values_[8]); + EXPECT_EQ(imu_msg.linear_acceleration.z, sensor_values_[9]); + + for (size_t i = 0; i < 9; ++i) + { + EXPECT_EQ(imu_msg.orientation_covariance[i], 0.0); + EXPECT_EQ(imu_msg.angular_velocity_covariance[i], 0.0); + EXPECT_EQ(imu_msg.linear_acceleration_covariance[i], 0.0); + } } From 908b7cc7ed5413d3fbb09203fd5edc4fc24abdd0 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sat, 22 Oct 2022 06:54:18 +0900 Subject: [PATCH 163/524] [doc] Remove Controllers which is redundant with Available Controllers (#456) --- doc/controllers_index.rst | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 23feac42d7..dee9d31abe 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -20,18 +20,10 @@ The controllers' namespaces are commanding the following command interface types - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - ... - -Controllers -*********** - -The following standard controllers are implemented: - - - `Joint Trajectory Controller `_ - provided a list of waypoints or target point defined with position, velocity and acceleration, the controller interpolates joint trajectories through it. - - ... ... - .. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp + Guidelines and Best Practices ***************************** @@ -43,7 +35,7 @@ Guidelines and Best Practices Available Controllers -===================== +********************* .. toctree:: :titlesonly: @@ -57,7 +49,7 @@ Available Controllers Available Broadcasters -====================== +********************** .. toctree:: :titlesonly: From 5c0cb9210b2230347af12bbe2690032f6942435d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 24 Oct 2022 09:48:11 +0200 Subject: [PATCH 164/524] Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (#371) --- diff_drive_controller/doc/userdoc.rst | 132 ++++++++++++++++++++++++++ 1 file changed, 132 insertions(+) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index fee26db2cd..dc158d7ff4 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -24,3 +24,135 @@ Other features Odometry publishing Task-space velocity, acceleration and jerk limits Automatic stop after command time-out + + +ros2_control Interfaces +------------------------ + +References +,,,,,,,,,,, + + +States +,,,,,,, + + +Commands +,,,,,,,,, + + +ROS2 Interfaces +---------------- + +Subscribers +,,,,,,,,,,,, +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. + +~/cmd_vel_unstamped [geometry_msgs::msg::Twist] + +~/cmd_vel_out [] + + + + +Publishers +,,,,,,,,,,, +~/odom [] + +/tf + + +Services +,,,,,,,,, + + +Parameters +------------ + +Wheels +,,,,,,, +left_wheel_names [array] + Link names of the left side wheels. + +right_wheel_names [array] + Link names of the right side wheels. + +wheel_separation [double] + Shortest distance between the left and right wheels. + If this parameter is wrong, the robot will not behave correctly in curves. + +wheels_per_side [integer] + 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. + +wheel_radius [double] + Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. + If this parameter is wrong the robot will move faster or slower then expected. + +wheel_separation_multiplier [double] + Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly) + +left_wheel_radius_multiplier [double] + Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter. + +right_wheel_radius_multiplier [double] + Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter. + +Odometry +,,,,,,,,, +base_frame_id [string] (default: "base_link") + Name of the frame for odometry. + This frame is parent of ``base_frame_id`` when controller publishes odometry. + +odom_frame_id [string] (default: "odom") + Name of the robot's base frame that is child of the odometry frame. + +pose_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") + Odometry covariance for the encoder output of the robot for the pose. + These values should be tuned to your robot's sample odometry data, but these values are a good place to start: + ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. + +twist_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") + Odometry covariance for the encoder output of the robot for the speed. + These values should be tuned to your robot's sample odometry data, but these values are a good place to start: + ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. + +open_loop [bool] (default: "false") + If set to false the odometry of the robot will be calculated from the commanded values and not from feedback. + +position_feedback [bool] (default: "true") + Is there position feedback from hardware. + +enable_odom_tf [bool] (default: "true") + Publish transformation between ``odom_frame_id`` and ``base_frame_id``. + +velocity_rolling_window_size [int] (default: "10") + (TODO(destogl): Please help me describe this correctly) + +Commands +``````````` +cmd_vel_timeout [double] (default: "0.5s") + Timeout after which input command on ``cmd_vel`` topic is considered staled. + +publish_limited_velocity [double] (default: "false") + Publish limited velocity value. + + +use_stamped_vel [bool] (default: "true") + Use stamp from input velocity message to calculate how old the command actually is. + +linear.x [JointLimits structure] + Joint limits structure for the linear X-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z [JointLimits structure] + Joint limits structure for the rotation about Z-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +publish_rate [double] (default: "50.0") + Publishing rate (Hz) of the odometry and TF messages. From a238100114f626ee17411673a87e8ab41863d6c9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 1 Nov 2022 01:46:35 -0600 Subject: [PATCH 165/524] Generate parameters for Gripper Action (#398) --- gripper_controllers/CMakeLists.txt | 41 +++++++---- .../gripper_action_controller.hpp | 10 +-- .../gripper_action_controller_impl.hpp | 69 ++++++++----------- gripper_controllers/package.xml | 1 + .../gripper_action_controller_parameters.yaml | 43 ++++++++++++ 5 files changed, 102 insertions(+), 62 deletions(-) create mode 100644 gripper_controllers/src/gripper_action_controller_parameters.yaml diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 40ca2fdd32..aa203709a0 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -17,14 +17,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp_action control_msgs control_toolbox controller_interface + generate_parameter_library hardware_interface pluginlib - realtime_tools rclcpp + rclcpp_action + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -32,6 +33,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) @@ -40,18 +45,10 @@ target_include_directories(gripper_action_controller PRIVATE $ PRIVATE $ ) -pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include -) - -install(TARGETS gripper_action_controller - LIBRARY DESTINATION lib +target_link_libraries(gripper_action_controller + gripper_action_controller_parameters ) +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -68,4 +65,22 @@ if(BUILD_TESTING) ) endif() +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gripper_action_controller + gripper_action_controller_parameters + EXPORT export_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index d5999002f4..78120b0cf5 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -43,6 +43,7 @@ #include "realtime_tools/realtime_server_goal_handle.h" // Project +#include "gripper_action_controller_parameters.hpp" #include "gripper_controllers/hardware_interface_adapter.hpp" namespace gripper_action_controller @@ -130,7 +131,8 @@ class GripperActionController : public controller_interface::ControllerInterface std::experimental::optional> joint_velocity_state_interface_; - std::string joint_name_; ///< Controlled joint names. + std::shared_ptr param_listener_; + Params params_; HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. @@ -158,12 +160,6 @@ class GripperActionController : public controller_interface::ControllerInterface rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time double computed_command_; ///< Computed command - double stall_timeout_, - stall_velocity_threshold_; ///< Stall related parameters - double default_max_effort_; ///< Max allowed effort - double goal_tolerance_; - bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful - /** * \brief Check for success and publish appropriate result and feedback. **/ diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 6fb9d61359..85f28d102b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -41,14 +41,8 @@ controller_interface::CallbackReturn GripperActionController: { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("action_monitor_rate", 20.0); - joint_name_ = auto_declare("joint", joint_name_); - auto_declare("goal_tolerance", 0.01); - auto_declare("max_effort", 0.0); - auto_declare("allow_stalling", false); - auto_declare("stall_velocity_threshold", 0.001); - auto_declare("stall_timeout", 1.0); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -145,7 +139,7 @@ template void GripperActionController::set_hold_position() { command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = default_max_effort_; + command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -159,7 +153,7 @@ void GripperActionController::check_for_success( return; } - if (fabs(error_position) < goal_tolerance_) + if (fabs(error_position) < params_.goal_tolerance) { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; @@ -171,17 +165,18 @@ void GripperActionController::check_for_success( } else { - if (fabs(current_velocity) > stall_velocity_threshold_) + if (fabs(current_velocity) > params_.stall_velocity_threshold) { last_movement_time_ = time; } - else if ((time - last_movement_time_).seconds() > stall_timeout_) + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; - if (allow_stalling_) + + if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); rt_active_goal_->setSucceeded(pre_alloc_result_); @@ -201,35 +196,25 @@ controller_interface::CallbackReturn GripperActionController: const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); // Action status checking update rate - const auto action_monitor_rate = get_node()->get_parameter("action_monitor_rate").as_double(); - action_monitor_period_ = rclcpp::Duration::from_seconds( - 1.0 / get_node()->get_parameter("action_monitor_rate").as_double()); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); // Controlled joint - joint_name_ = get_node()->get_parameter("joint").as_string(); - if (joint_name_.empty()) + if (params_.joint.empty()) { RCLCPP_ERROR(logger, "Could not find joint name on param server"); return controller_interface::CallbackReturn::ERROR; } - // Default tolerances - goal_tolerance_ = get_node()->get_parameter("goal_tolerance").as_double(); - goal_tolerance_ = fabs(goal_tolerance_); - // Max allowable effort - default_max_effort_ = get_node()->get_parameter("max_effort").as_double(); - default_max_effort_ = fabs(default_max_effort_); - // Allow stalling will make the action server return success if the - // gripper stalls when moving to the goal - allow_stalling_ = get_node()->get_parameter("allow_stalling").as_bool(); - // Stall - stall velocity threshold, stall timeout - stall_velocity_threshold_ = get_node()->get_parameter("stall_velocity_threshold").as_double(); - stall_timeout_ = get_node()->get_parameter("stall_timeout").as_double(); - return controller_interface::CallbackReturn::SUCCESS; } template @@ -245,12 +230,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_prefix_name() != joint_name_) + if (position_command_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position command interface is different than joint name `" << position_command_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -262,12 +247,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_state_interface_it->get_prefix_name() != joint_name_) + if (position_state_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position state interface is different than joint name `" << position_state_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -279,12 +264,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); return controller_interface::CallbackReturn::ERROR; } - if (velocity_state_interface_it->get_prefix_name() != joint_name_) + if (velocity_state_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Velocity command interface is different than joint name `" << velocity_state_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -297,7 +282,7 @@ controller_interface::CallbackReturn GripperActionController: // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = default_max_effort_; + command_struct_.max_effort_ = params_.max_effort; command_.initRT(command_struct_); // Result @@ -334,7 +319,7 @@ GripperActionController::command_interface_configuration() co { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {joint_name_ + "/" + hardware_interface::HW_IF_POSITION}}; + {params_.joint + "/" + hardware_interface::HW_IF_POSITION}}; } template @@ -343,8 +328,8 @@ GripperActionController::state_interface_configuration() cons { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {joint_name_ + "/" + hardware_interface::HW_IF_POSITION, - joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}}; + {params_.joint + "/" + hardware_interface::HW_IF_POSITION, + params_.joint + "/" + hardware_interface::HW_IF_VELOCITY}}; } template diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 1f5ae7ceba..46916ba584 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -17,6 +17,7 @@ control_msgs control_toolbox controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_action diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9027e5b730 --- /dev/null +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,43 @@ +gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + validation: { + lower_bounds: [0.1] + }, + } + joint: { + type: string, + default_value: "", + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + lower_bounds: [0.0] + }, + } + max_effort: { + type: double, + default_value: 0.0, + description: "Max allowable effort", + validation: { + lower_bounds: [0.0] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + } From 12160de6d9c132b99b6c6f1cf975f6296006db7c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Nov 2022 18:18:33 +0000 Subject: [PATCH 166/524] Use optional from C++17 (#460) --- .../gripper_action_controller.hpp | 8 +++----- .../gripper_action_controller_impl.hpp | 6 +++--- .../hardware_interface_adapter.hpp | 16 +++++----------- 3 files changed, 11 insertions(+), 19 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 78120b0cf5..c219c7bc09 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -22,8 +22,6 @@ #include #include #include -// TODO(JafarAbdi): Remove experimental once the default standard is C++17 -#include "experimental/optional" // ROS #include "rclcpp/rclcpp.hpp" @@ -124,11 +122,11 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. - std::experimental::optional> + std::optional> joint_position_command_interface_; - std::experimental::optional> + std::optional> joint_position_state_interface_; - std::experimental::optional> + std::optional> joint_velocity_state_interface_; std::shared_ptr param_listener_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 85f28d102b..423cf0861a 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -306,9 +306,9 @@ template controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_position_command_interface_ = std::experimental::nullopt; - joint_position_state_interface_ = std::experimental::nullopt; - joint_velocity_state_interface_ = std::experimental::nullopt; + joint_position_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; release_interfaces(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 40fde4442c..97ebbb1f4b 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -23,8 +23,6 @@ #include #include #include -// TODO(JafarAbdi): Remove experimental once the default standard is C++17 -#include "experimental/optional" #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -44,7 +42,7 @@ class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional< + std::optional< std::reference_wrapper> /* joint_handle */, std::shared_ptr & /* node */) { @@ -71,8 +69,7 @@ class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; @@ -92,8 +89,7 @@ class HardwareInterfaceAdapter } private: - std::experimental::optional> - joint_handle_; + std::optional> joint_handle_; }; /** @@ -130,8 +126,7 @@ class HardwareInterfaceAdapter } bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, const std::shared_ptr & node) { joint_handle_ = joint_handle; @@ -182,8 +177,7 @@ class HardwareInterfaceAdapter private: using PidPtr = std::shared_ptr; PidPtr pid_; - std::experimental::optional> - joint_handle_; + std::optional> joint_handle_; std::chrono::steady_clock::time_point last_update_time_; }; From 39fce8974df43c398754b93f986363e92e3e8b59 Mon Sep 17 00:00:00 2001 From: sp-sophia-labs <111747728+sp-sophia-labs@users.noreply.github.com> Date: Sat, 12 Nov 2022 11:34:12 +0100 Subject: [PATCH 167/524] Odom Topic & Frame Namespaces (#461) --- .../src/diff_drive_controller.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index bfb5baf3c3..c4fec1430a 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -471,6 +471,20 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); + std::string controller_namespace = std::string(get_node()->get_namespace()); + + if (controller_namespace == "/") + { + controller_namespace = ""; + } + else + { + controller_namespace = controller_namespace + "/"; + } + + odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id; + odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id; + auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; From 950fb26b41a899d64a11b3f310ad0c3e181afcf4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 16 Nov 2022 20:21:23 +0100 Subject: [PATCH 168/524] [AdmittanceController] Add missing dependecies for the tests (#465) We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. --- admittance_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index e352dcfa91..a1d1904c21 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -30,11 +30,11 @@ tf2_ros trajectory_msgs - ament_cmake_gmock control_msgs controller_manager hardware_interface + kinematics_interface_kdl ros2_control_test_assets From bc1ed47b50df3317a511a4539e7b7cae24ae937d Mon Sep 17 00:00:00 2001 From: light-tech Date: Fri, 18 Nov 2022 05:06:24 +0700 Subject: [PATCH 169/524] Include to fix compilation error on macOS (#467) --- tricycle_controller/src/steering_limiter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index c1649f12dc..b76cc0f602 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -18,6 +18,7 @@ #include #include +#include #include "rcppmath/clamp.hpp" #include "tricycle_controller/steering_limiter.hpp" From 14ea6ba3a3cd6bc1697c2d3e3054c77dca927809 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Nov 2022 23:41:15 +0000 Subject: [PATCH 170/524] Bring admittance_controller version up to speed --- admittance_controller/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a1d1904c21..3859520ae6 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,9 +2,10 @@ admittance_controller - 0.0.0 + 2.13.0 Implementation of admittance controllers for different input and output interface. Denis Štogl + Bence Magyar Andy Zelenak Apache License 2.0 From 933da0f19b9b86256209abb0e790f45100c31657 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Nov 2022 23:46:07 +0000 Subject: [PATCH 171/524] Add admittance_controller to ros2_controllers metapackage --- ros2_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 125988addf..e81ed2b798 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -5,12 +5,12 @@ Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios - Karsten Knese Apache License 2.0 ament_cmake + admittance_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster From a0fc29ef138d882acba943cfc6aae25c9341d140 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 18 Nov 2022 00:36:33 +0000 Subject: [PATCH 172/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 110 ++++++++++++++++++ diff_drive_controller/CHANGELOG.rst | 6 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 5 + gripper_controllers/CHANGELOG.rst | 6 + imu_sensor_broadcaster/CHANGELOG.rst | 6 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 5 + position_controllers/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 5 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 5 + velocity_controllers/CHANGELOG.rst | 3 + 15 files changed, 171 insertions(+) create mode 100644 admittance_controller/CHANGELOG.rst diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst new file mode 100644 index 0000000000..d8571f6303 --- /dev/null +++ b/admittance_controller/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package admittance_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Bring admittance_controller version up to speed +* [AdmittanceController] Add missing dependecies for the tests (`#465 `_) + We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. +* Fix parameter library export (`#448 `_) +* Add generic admittance controller for TCP wrenches (`#370 `_) + Co-authored-by: AndyZe + Co-authored-by: Denis Štogl +* Contributors: Bence Magyar, Denis Štogl, Paul Gesel, Tyler Weaver + +* Bring admittance_controller version up to speed +* [AdmittanceController] Add missing dependecies for the tests (`#465 `_) + We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. +* Fix parameter library export (`#448 `_) +* Add generic admittance controller for TCP wrenches (`#370 `_) + Co-authored-by: AndyZe + Co-authored-by: Denis Štogl +* Contributors: Bence Magyar, Denis Štogl, Paul Gesel, Tyler Weaver + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3c2714dc64..eb98ef30ee 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Odom Topic & Frame Namespaces (`#461 `_) +* Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (`#371 `_) +* Contributors: Denis Štogl, sp-sophia-labs + 2.13.0 (2022-10-05) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index aeec4541f4..0778813310 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7e285a6ac0..dee110d588 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix parameter library export (`#448 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 19b4711223..7b429662ca 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate params for ForwardCommandController (`#396 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ee226c598f..dc6f15552f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use optional from C++17 (`#460 `_) +* Generate parameters for Gripper Action (`#398 `_) +* Contributors: Bence Magyar, Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 285d7e061d..eabe61732d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) +* Generate parameters for IMU Sensor Broadcaster (`#399 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.13.0 (2022-10-05) ------------------- * Fix undeclared and wrong parameters in controllers. (`#438 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9dc2a70a9d..1c95a97006 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- * Generate parameters for Joint State Broadcaster (`#401 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f7244e062d..0bec81752e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix parameter library export (`#448 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- * Generate Parameter Library for Joint Trajectory Controller (`#384 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d78bfc246b..218646366f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0c194f3db8..38bd425c35 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b791ac2a81..26196e3652 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 +----------- +* Remove deprecation warning when parameter without value is set. (`#445 `_) +* Contributors: Denis Štogl + 2.13.0 (2022-10-05) ------------------- * Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 130d482684..43faf36e42 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c71becc1ce..e1195f4ebb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Include to fix compilation error on macOS (`#467 `_) +* Contributors: light-tech + 2.13.0 (2022-10-05) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cdabd8ca5e..45f0ce49fb 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- From 755fec9580cdb74f95b443216eb35a1381226397 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 18 Nov 2022 00:37:18 +0000 Subject: [PATCH 173/524] 2.14.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index d8571f6303..1b09c945b6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Bring admittance_controller version up to speed * [AdmittanceController] Add missing dependecies for the tests (`#465 `_) We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 3859520ae6..34f7ba23b2 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.13.0 + 2.14.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index eb98ef30ee..f631cb18c6 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Odom Topic & Frame Namespaces (`#461 `_) * Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (`#371 `_) * Contributors: Denis Štogl, sp-sophia-labs diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 474ff1a2d8..764f52c66d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.13.0 + 2.14.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 0778813310..95d292e041 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index f9164998db..b1c174f2c1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.13.0 + 2.14.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 dee110d588..d31b8cc6a8 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Fix parameter library export (`#448 `_) * Contributors: Tyler Weaver diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 0e105ceb2c..75f2cc70dc 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.13.0 + 2.14.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 7b429662ca..04b740e499 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Generate params for ForwardCommandController (`#396 `_) * Contributors: Tyler Weaver diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 47e12892ca..0510e8bb3a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index dc6f15552f..c4ff69decf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Use optional from C++17 (`#460 `_) * Generate parameters for Gripper Action (`#398 `_) * Contributors: Bence Magyar, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 46916ba584..2f46a5c970 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.13.0 + 2.14.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index eabe61732d..c79e3bc3fc 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) * Generate parameters for IMU Sensor Broadcaster (`#399 `_) * Contributors: Denis Štogl, Tyler Weaver diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 855d3fe58e..6eba494c33 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.13.0 + 2.14.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 1c95a97006..44600171f6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 43687c70ea..a4523a9dc0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.13.0 + 2.14.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 0bec81752e..deb9d68c6e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Fix parameter library export (`#448 `_) * Contributors: Tyler Weaver diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f768c82a8f..f6c8984a66 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.13.0 + 2.14.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 218646366f..c199ae0ed0 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 02d9ca7cf4..4a435fe93c 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 38bd425c35..fcfbacc0e6 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e81ed2b798..5836915aa1 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.13.0 + 2.14.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 26196e3652..a4feb47161 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Remove deprecation warning when parameter without value is set. (`#445 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index a3810d560a..3367d44c2d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.13.0 + 2.14.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 fdc3646ad1..1f24996371 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.13.0", + version="2.14.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 43faf36e42..5a01e489c2 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 7529c38428..faff00589e 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.13.0 + 2.14.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 d1a720d311..d635d515b0 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.13.0", + version="2.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index e1195f4ebb..65cc206bda 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Include to fix compilation error on macOS (`#467 `_) * Contributors: light-tech diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 467ed03ffc..bd5204c357 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.13.0 + 2.14.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 45f0ce49fb..beb72450a1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index aecb6cf3d1..fe5dbdbb56 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 09e50139882b31a21e2c54ec1c4bf691bf58ef3d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 27 Nov 2022 18:58:32 +0100 Subject: [PATCH 174/524] Add basic gripper controller tests (#459) --- gripper_controllers/CMakeLists.txt | 9 + .../gripper_action_controller.hpp | 2 +- .../gripper_action_controller_impl.hpp | 2 +- .../test/test_gripper_controllers.cpp | 154 ++++++++++++++++++ .../test/test_gripper_controllers.hpp | 65 ++++++++ 5 files changed, 230 insertions(+), 2 deletions(-) create mode 100644 gripper_controllers/test/test_gripper_controllers.cpp create mode 100644 gripper_controllers/test/test_gripper_controllers.hpp diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index aa203709a0..97ccd2788d 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -63,6 +63,15 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock( + test_gripper_controllers + test/test_gripper_controllers.cpp + ) + target_include_directories(test_gripper_controllers PRIVATE include) + target_link_libraries(test_gripper_controllers + gripper_action_controller + ) endif() install( diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index c219c7bc09..830de7b514 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -107,7 +107,7 @@ class GripperActionController : public controller_interface::ControllerInterface // pre-allocated memory that is re-used to set the realtime buffer Commands command_struct_, command_struct_rt_; -private: +protected: using GripperCommandAction = control_msgs::action::GripperCommand; using ActionServer = rclcpp_action::Server; using ActionServerPtr = ActionServer::SharedPtr; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 423cf0861a..e28495fd1e 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -211,7 +211,7 @@ controller_interface::CallbackReturn GripperActionController: // Controlled joint if (params_.joint.empty()) { - RCLCPP_ERROR(logger, "Could not find joint name on param server"); + RCLCPP_ERROR(logger, "Joint name cannot be empty"); return controller_interface::CallbackReturn::ERROR; } diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp new file mode 100644 index 0000000000..4c82eb6961 --- /dev/null +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -0,0 +1,154 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_gripper_controllers.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::GripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; + +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +void GripperControllerTest::SetUpController() +{ + const auto result = controller_->init("gripper_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp new file mode 100644 index 0000000000..3e9750a603 --- /dev/null +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -0,0 +1,65 @@ +// Copyright 2022 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_GRIPPER_CONTROLLERS_HPP_ +#define TEST_GRIPPER_CONTROLLERS_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "gripper_controllers/gripper_action_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::StateInterface; + +// subclassing and friending so we can access member variables +class FriendGripperController +: public gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; + StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; + CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; +}; + +#endif // TEST_GRIPPER_CONTROLLERS_HPP_ From bc7be0ee888c71662bca67eb5b57e2c5d9699a68 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 3 Dec 2022 08:07:13 +0000 Subject: [PATCH 175/524] Fix format step (#475) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index fba9f85911..dd78cca0ed 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -14,7 +14,7 @@ jobs: - uses: actions/checkout@v3 - uses: actions/setup-python@v2 with: - python-version: 3.9.7 + python-version: 3.9 - name: Install system hooks run: sudo apt install -qq clang-format-12 cppcheck - uses: pre-commit/action@v2.0.3 From 5a367e25a310db6adbd7657539a7464af79e7d62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maciej=20St=C4=99pie=C5=84?= Date: Sat, 3 Dec 2022 13:11:45 +0100 Subject: [PATCH 176/524] [DiffDriveController] Change units of velocity feedback (#452) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds multiplying feedback by wheel radius so that it can be in rad/s (which should be expected unit) Co-authored-by: Maciej Stępień Co-authored-by: Bence Magyar Co-authored-by: Denis Štogl --- diff_drive_controller/src/diff_drive_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c4fec1430a..e873aeb4f0 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -220,7 +220,8 @@ controller_interface::return_type DiffDriveController::update( else { odometry_.updateFromVelocity( - left_feedback_mean * period.seconds(), right_feedback_mean * period.seconds(), time); + left_feedback_mean * left_wheel_radius * period.seconds(), + right_feedback_mean * right_wheel_radius * period.seconds(), time); } } From bc88acbf8a2bc15030a00ab17aa4065a3bce304a Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 4 Dec 2022 04:02:52 -0500 Subject: [PATCH 177/524] [DiffDriveController] Use generate parameter library (#386) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- diff_drive_controller/CMakeLists.txt | 9 + diff_drive_controller/doc/userdoc.rst | 78 +------ .../diff_drive_controller.hpp | 34 +-- diff_drive_controller/package.xml | 1 + .../src/diff_drive_controller.cpp | 198 +++++------------- .../src/diff_drive_controller_parameter.yaml | 177 ++++++++++++++++ 6 files changed, 250 insertions(+), 247 deletions(-) create mode 100644 diff_drive_controller/src/diff_drive_controller_parameter.yaml diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 09ca27e1d2..83f18a9931 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -30,6 +30,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(diff_drive_controller_parameters + src/diff_drive_controller_parameter.yaml +) + add_library(${PROJECT_NAME} SHARED src/diff_drive_controller.cpp src/odometry.cpp @@ -39,6 +45,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} + diff_drive_controller_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index dc158d7ff4..1ef47bd975 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -70,79 +70,10 @@ Services Parameters ------------ -Wheels -,,,,,,, -left_wheel_names [array] - Link names of the left side wheels. - -right_wheel_names [array] - Link names of the right side wheels. - -wheel_separation [double] - Shortest distance between the left and right wheels. - If this parameter is wrong, the robot will not behave correctly in curves. - -wheels_per_side [integer] - 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. - -wheel_radius [double] - Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. - If this parameter is wrong the robot will move faster or slower then expected. - -wheel_separation_multiplier [double] - Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly) - -left_wheel_radius_multiplier [double] - Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter. - -right_wheel_radius_multiplier [double] - Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter. - -Odometry -,,,,,,,,, -base_frame_id [string] (default: "base_link") - Name of the frame for odometry. - This frame is parent of ``base_frame_id`` when controller publishes odometry. - -odom_frame_id [string] (default: "odom") - Name of the robot's base frame that is child of the odometry frame. +Check `parameter definition file for details `_. -pose_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the pose. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -twist_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the speed. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -open_loop [bool] (default: "false") - If set to false the odometry of the robot will be calculated from the commanded values and not from feedback. - -position_feedback [bool] (default: "true") - Is there position feedback from hardware. - -enable_odom_tf [bool] (default: "true") - Publish transformation between ``odom_frame_id`` and ``base_frame_id``. - -velocity_rolling_window_size [int] (default: "10") - (TODO(destogl): Please help me describe this correctly) - -Commands -``````````` -cmd_vel_timeout [double] (default: "0.5s") - Timeout after which input command on ``cmd_vel`` topic is considered staled. - -publish_limited_velocity [double] (default: "false") - Publish limited velocity value. - - -use_stamped_vel [bool] (default: "true") - Use stamp from input velocity message to calculate how old the command actually is. +Note that the documentation on parameters for joint limits can be found in `their header file `_. +Those parameters are: linear.x [JointLimits structure] Joint limits structure for the linear X-axis. @@ -153,6 +84,3 @@ angular.z [JointLimits structure] Joint limits structure for the rotation about Z-axis. The limiter ignores position limits. For details see ``joint_limits`` package from ros2_control repository. - -publish_rate [double] (default: "50.0") - Publishing rate (Hz) of the odometry and TF messages. 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 ce357e4116..f229667fb9 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 @@ -42,6 +42,8 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" +#include "diff_drive_controller_parameters.hpp" + namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface @@ -101,35 +103,18 @@ class DiffDriveController : public controller_interface::ControllerInterface const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); - std::vector left_wheel_names_; - std::vector right_wheel_names_; - std::vector registered_left_wheel_handles_; std::vector registered_right_wheel_handles_; - struct WheelParams - { - size_t wheels_per_side = 0; - double separation = 0.0; // w.r.t. the midpoint of the wheel width - double radius = 0.0; // Assumed to be the same for both wheels - double separation_multiplier = 1.0; - double left_radius_multiplier = 1.0; - double right_radius_multiplier = 1.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool position_feedback = true; - bool enable_odom_tf = true; - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; + // Parameters from ROS for diff_drive_controller + std::shared_ptr param_listener_; + Params params_; Odometry odometry_; + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -139,9 +124,6 @@ class DiffDriveController : public controller_interface::ControllerInterface std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 764f52c66d..6b675ccc10 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake + generate_parameter_library controller_interface geometry_msgs diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e873aeb4f0..75d8b1fa35 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -50,58 +50,16 @@ DiffDriveController::DiffDriveController() : controller_interface::ControllerInt const char * DiffDriveController::feedback_type() const { - return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; } controller_interface::CallbackReturn DiffDriveController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare>("left_wheel_names", std::vector()); - auto_declare>("right_wheel_names", std::vector()); - - auto_declare("wheel_separation", wheel_params_.separation); - auto_declare("wheels_per_side", wheel_params_.wheels_per_side); - auto_declare("wheel_radius", wheel_params_.radius); - auto_declare("wheel_separation_multiplier", wheel_params_.separation_multiplier); - auto_declare("left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); - auto_declare("right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("position_feedback", odom_params_.position_feedback); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - publish_limited_velocity_ = - auto_declare("publish_limited_velocity", publish_limited_velocity_); - auto_declare("velocity_rolling_window_size", 10); - use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("linear.x.has_velocity_limits", false); - auto_declare("linear.x.has_acceleration_limits", false); - auto_declare("linear.x.has_jerk_limits", false); - auto_declare("linear.x.max_velocity", NAN); - auto_declare("linear.x.min_velocity", NAN); - auto_declare("linear.x.max_acceleration", NAN); - auto_declare("linear.x.min_acceleration", NAN); - auto_declare("linear.x.max_jerk", NAN); - auto_declare("linear.x.min_jerk", NAN); - - auto_declare("angular.z.has_velocity_limits", false); - auto_declare("angular.z.has_acceleration_limits", false); - auto_declare("angular.z.has_jerk_limits", false); - auto_declare("angular.z.max_velocity", NAN); - auto_declare("angular.z.min_velocity", NAN); - auto_declare("angular.z.max_acceleration", NAN); - auto_declare("angular.z.min_acceleration", NAN); - auto_declare("angular.z.max_jerk", NAN); - auto_declare("angular.z.min_jerk", NAN); - publish_rate_ = auto_declare("publish_rate", publish_rate_); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -115,11 +73,11 @@ controller_interface::CallbackReturn DiffDriveController::on_init() InterfaceConfiguration DiffDriveController::command_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } @@ -129,11 +87,11 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co InterfaceConfiguration DiffDriveController::state_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } @@ -180,12 +138,11 @@ controller_interface::return_type DiffDriveController::update( previous_update_timestamp_ = time; // Apply (possibly new) multipliers: - const auto wheels = wheel_params_; - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, time); } @@ -193,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -210,10 +167,10 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= wheels.wheels_per_side; - right_feedback_mean /= wheels.wheels_per_side; + left_feedback_mean /= params_.wheels_per_side; + right_feedback_mean /= params_.wheels_per_side; - if (odom_params_.position_feedback) + if (params_.position_feedback) { odometry_.update(left_feedback_mean, right_feedback_mean, time); } @@ -247,7 +204,7 @@ controller_interface::return_type DiffDriveController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -287,7 +244,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -301,100 +258,50 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { auto logger = get_node()->get_logger(); - // update parameters - left_wheel_names_ = get_node()->get_parameter("left_wheel_names").as_string_array(); - right_wheel_names_ = get_node()->get_parameter("right_wheel_names").as_string_array(); + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } - if (left_wheel_names_.size() != right_wheel_names_.size()) + if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) { RCLCPP_ERROR( logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", - left_wheel_names_.size(), right_wheel_names_.size()); + params_.left_wheel_names.size(), params_.right_wheel_names.size()); return controller_interface::CallbackReturn::ERROR; } - if (left_wheel_names_.empty()) + if (params_.left_wheel_names.empty()) { RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); return controller_interface::CallbackReturn::ERROR; } - wheel_params_.separation = get_node()->get_parameter("wheel_separation").as_double(); - wheel_params_.wheels_per_side = - static_cast(get_node()->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - wheel_params_.separation_multiplier = - get_node()->get_parameter("wheel_separation_multiplier").as_double(); - wheel_params_.left_radius_multiplier = - get_node()->get_parameter("left_wheel_radius_multiplier").as_double(); - wheel_params_.right_radius_multiplier = - get_node()->get_parameter("right_wheel_radius_multiplier").as_double(); - - const auto wheels = wheel_params_; - - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); - - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.position_feedback = get_node()->get_parameter("position_feedback").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); cmd_vel_timeout_ = std::chrono::milliseconds{ static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - try - { - limiter_linear_ = SpeedLimiter( - get_node()->get_parameter("linear.x.has_velocity_limits").as_bool(), - get_node()->get_parameter("linear.x.has_acceleration_limits").as_bool(), - get_node()->get_parameter("linear.x.has_jerk_limits").as_bool(), - get_node()->get_parameter("linear.x.min_velocity").as_double(), - get_node()->get_parameter("linear.x.max_velocity").as_double(), - get_node()->get_parameter("linear.x.min_acceleration").as_double(), - get_node()->get_parameter("linear.x.max_acceleration").as_double(), - get_node()->get_parameter("linear.x.min_jerk").as_double(), - get_node()->get_parameter("linear.x.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring linear speed limiter: %s", e.what()); - } + limiter_linear_ = SpeedLimiter( + params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, + params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, + params_.linear.x.max_jerk); - try - { - limiter_angular_ = SpeedLimiter( - get_node()->get_parameter("angular.z.has_velocity_limits").as_bool(), - get_node()->get_parameter("angular.z.has_acceleration_limits").as_bool(), - get_node()->get_parameter("angular.z.has_jerk_limits").as_bool(), - get_node()->get_parameter("angular.z.min_velocity").as_double(), - get_node()->get_parameter("angular.z.max_velocity").as_double(), - get_node()->get_parameter("angular.z.min_acceleration").as_double(), - get_node()->get_parameter("angular.z.max_acceleration").as_double(), - get_node()->get_parameter("angular.z.min_jerk").as_double(), - get_node()->get_parameter("angular.z.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); - } + limiter_angular_ = SpeedLimiter( + params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, + params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, + params_.angular.z.max_velocity, params_.angular.z.min_acceleration, + params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); if (!reset()) { @@ -402,7 +309,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - wheel_params_.wheels_per_side = left_wheel_names_.size(); + params_.wheels_per_side = params_.left_wheel_names.size(); if (publish_limited_velocity_) { @@ -483,12 +390,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( controller_namespace = controller_namespace + "/"; } - odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id; - odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id; + const auto odom_frame_id = controller_namespace + params_.odom_frame_id; + const auto base_frame_id = controller_namespace + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = controller_namespace + odom_frame_id; + odometry_message.child_frame_id = controller_namespace + base_frame_id; // limit the publication on the topics /odom and /tf publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); @@ -504,9 +411,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -519,8 +425,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; @@ -530,9 +436,9 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( const rclcpp_lifecycle::State &) { const auto left_result = - configure_side("left", left_wheel_names_, registered_left_wheel_handles_); + configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_); const auto right_result = - configure_side("right", right_wheel_names_, registered_right_wheel_handles_); + configure_side("right", params_.right_wheel_names, registered_right_wheel_handles_); if ( left_result == controller_interface::CallbackReturn::ERROR || diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml new file mode 100644 index 0000000000..82ba210113 --- /dev/null +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -0,0 +1,177 @@ +diff_drive_controller: + left_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the left side wheels", + } + right_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the right side wheels", + } + wheel_separation: { + type: double, + default_value: 0.0, + description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + } + 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.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)", + } + left_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + right_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "odom", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + cmd_vel_timeout: { + type: double, + default_value: 0.5, # seconds + description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_limited_velocity: { + type: bool, + default_value: false, + description: "Publish limited velocity value.", + } + velocity_rolling_window_size: { + type: int, + 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 + description: "Publishing rate (Hz) of the odometry and TF messages.", + } + linear: + x: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + angular: + z: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } From 4c0d58e459eb2352c14c9eeffad04d4d8a4100e8 Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Sun, 4 Dec 2022 20:49:07 +0100 Subject: [PATCH 178/524] =?UTF-8?q?[TricycleController]=20Removed=20?= =?UTF-8?q?=E2=80=9Cpublish=20period=E2=80=9D=20functionality=20=E2=8F=B1?= =?UTF-8?q?=20#abi-break=20#behavior-break=20(#468)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../tricycle_controller.hpp | 5 -- .../src/tricycle_controller.cpp | 61 ++++++++----------- 2 files changed, 25 insertions(+), 41 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 2178008725..cef90d026a 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -167,11 +167,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - // publish rate limiter - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; - bool is_halted = false; bool use_stamped_vel_ = true; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 114731242c..4f4e22ec9d 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -71,7 +71,6 @@ CallbackReturn TricycleController::on_init() auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); - auto_declare("publish_rate", publish_rate_); auto_declare("traction.max_velocity", NAN); auto_declare("traction.min_velocity", NAN); @@ -169,40 +168,35 @@ controller_interface::return_type TricycleController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < time) + if (realtime_odometry_publisher_->trylock()) { - previous_publish_timestamp_ += publish_period_; - - if (realtime_odometry_publisher_->trylock()) + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) - { - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - } - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) - { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); - } + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); } // Compute wheel velocity and angle @@ -305,9 +299,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - try { limiter_traction_ = TractionLimiter( @@ -421,8 +412,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; - previous_publish_timestamp_ = get_node()->get_clock()->now(); - // initialize odom values zeros odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); From 091ab8d1c350c7fc879d59e310ca9cf197c35891 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Dec 2022 12:22:15 +0000 Subject: [PATCH 179/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 52 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1b09c945b6..a69b0412d9 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Bring admittance_controller version up to speed diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index f631cb18c6..2c1a4d510d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [DiffDriveController] Use generate parameter library (`#386 `_) +* [DiffDriveController] Change units of velocity feedback (`#452 `_) +* Contributors: Maciej Stępień, Paul Gesel, Denis Štogl, Bence Magyar + 2.14.0 (2022-11-18) ------------------- * Odom Topic & Frame Namespaces (`#461 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 95d292e041..cacea16444 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d31b8cc6a8..a371221642 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Fix parameter library export (`#448 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04b740e499..3ca94cb835 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Generate params for ForwardCommandController (`#396 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c4ff69decf..ee20bdc4f5 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add basic gripper controller tests (`#459 `_) +* Contributors: Bence Magyar + 2.14.0 (2022-11-18) ------------------- * Use optional from C++17 (`#460 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c79e3bc3fc..9b5d205275 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 44600171f6..16c4735717 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index deb9d68c6e..1e2d50a3b8 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Fix parameter library export (`#448 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c199ae0ed0..b3a5fcdd0d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index fcfbacc0e6..05756ee7b2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a4feb47161..0b4d93973b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Remove deprecation warning when parameter without value is set. (`#445 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5a01e489c2..3d1869a9d9 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 65cc206bda..b235abcb52 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) +* Contributors: Robotgir, Denis Štogl + 2.14.0 (2022-11-18) ------------------- * Include to fix compilation error on macOS (`#467 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index beb72450a1..00101fc622 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- From f08c3789e81b72f64858eccfb4a1553fd9ed801f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Dec 2022 12:23:30 +0000 Subject: [PATCH 180/524] 2.15.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a69b0412d9..cc7fa5f2bf 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 34f7ba23b2..876ed424b0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.14.0 + 2.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 2c1a4d510d..3546af9bba 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) * [DiffDriveController] Change units of velocity feedback (`#452 `_) * Contributors: Maciej Stępień, Paul Gesel, Denis Štogl, Bence Magyar diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 6b675ccc10..8b2e368824 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.14.0 + 2.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index cacea16444..6954b1f7e3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index b1c174f2c1..7476889b73 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a371221642..3a37a17174 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 75f2cc70dc..8cee3ef6a3 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.14.0 + 2.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3ca94cb835..0dc1db853b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0510e8bb3a..7853d78542 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ee20bdc4f5..e11321aefe 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * Add basic gripper controller tests (`#459 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2f46a5c970..b033e99b12 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.14.0 + 2.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9b5d205275..0f26b4cbf2 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6eba494c33..79eb5e14a7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.14.0 + 2.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 16c4735717..68d25c05aa 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a4523a9dc0..c24d274a93 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.14.0 + 2.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1e2d50a3b8..bc59ab4960 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f6c8984a66..3e806ce59e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.14.0 + 2.15.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 b3a5fcdd0d..115fac4e37 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 4a435fe93c..21cdd59b86 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 05756ee7b2..0799e154c7 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5836915aa1..e91679802c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.14.0 + 2.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0b4d93973b..f60cb9d72a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 3367d44c2d..269d1da466 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.14.0 + 2.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 1f24996371..ed3ae63656 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.14.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 3d1869a9d9..d4b07df08b 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index faff00589e..81e69f88fd 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.14.0 + 2.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index d635d515b0..afb93527f1 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.14.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index b235abcb52..0739bf6446 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) * Contributors: Robotgir, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index bd5204c357..1681367c25 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.14.0 + 2.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 00101fc622..e47e52cdac 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index fe5dbdbb56..131bcd11f0 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 54f2c598e7305f06080d941ffe52e4ce5a2d67ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 15:51:14 +0100 Subject: [PATCH 181/524] Remove compilation warnings from DiffDriveController (#477) --- .../include/diff_drive_controller/odometry.hpp | 4 ++-- diff_drive_controller/src/speed_limiter.cpp | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 2adc512f88..83ab270f72 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -25,7 +25,7 @@ #include #include "rclcpp/time.hpp" -#include "rcppmath/rolling_mean_accumulator.hpp" +#include "rcpputils/rolling_mean_accumulator.hpp" namespace diff_drive_controller { @@ -50,7 +50,7 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp index e25a6f47ed..0f82c2cc3b 100644 --- a/diff_drive_controller/src/speed_limiter.cpp +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -20,7 +20,6 @@ #include #include "diff_drive_controller/speed_limiter.hpp" -#include "rcppmath/clamp.hpp" namespace diff_drive_controller { @@ -92,7 +91,7 @@ double SpeedLimiter::limit_velocity(double & v) if (has_velocity_limits_) { - v = rcppmath::clamp(v, min_velocity_, max_velocity_); + v = std::clamp(v, min_velocity_, max_velocity_); } return tmp != 0.0 ? v / tmp : 1.0; @@ -107,7 +106,7 @@ double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) const double dv_min = min_acceleration_ * dt; const double dv_max = max_acceleration_ * dt; - const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + const double dv = std::clamp(v - v0, dv_min, dv_max); v = v0 + dv; } @@ -129,7 +128,7 @@ double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + const double da = std::clamp(dv - dv0, da_min, da_max); v = v0 + dv0 + da; } From 4b9c06a0ef6c21b0cd3bb3e1b0d25c915a8f9e7f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 15:52:55 +0100 Subject: [PATCH 182/524] Fix deprecation warnings when compiling (#478) --- tricycle_controller/include/tricycle_controller/odometry.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 4a958a93c6..62eb51d6cc 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -22,7 +22,7 @@ #include #include "rclcpp/time.hpp" -#include "rcppmath/rolling_mean_accumulator.hpp" +#include "rcpputils/rolling_mean_accumulator.hpp" namespace tricycle_controller { @@ -45,7 +45,7 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); From b72d0bbc97ea6cf4a1580c4bb734d7879350d2f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 20:41:44 +0100 Subject: [PATCH 183/524] [JTC] Remove deprecation from parameters validation file. (#476) * [JTC] Remove deprecation from parameters validation file. * Add new dependencies. * Add missing dependencies * Update package.xml --- joint_trajectory_controller/CMakeLists.txt | 2 + .../validate_jtc_parameters.hpp | 49 +++++++++++-------- joint_trajectory_controller/package.xml | 4 +- 3 files changed, 34 insertions(+), 21 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 32461617ac..f3f8c2ab7b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -22,6 +22,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + rsl + tl_expected trajectory_msgs ) 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 09363eebb1..fc6319fabf 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 @@ -16,12 +16,17 @@ #define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ #include +#include #include "parameter_traits/parameter_traits.hpp" +#include "rclcpp/parameter.hpp" +#include "rsl/algorithm.hpp" +#include "tl_expected/expected.hpp" namespace parameter_traits { -Result command_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected command_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -31,33 +36,37 @@ Result command_interface_type_combinations(rclcpp::Parameter const & parameter) // 2. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && interface_types.size() > 1 && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' command interface can be used either alone or 'position' " "interface has to be present"); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "velocity") && - !contains(interface_types, "position"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' command interface can only be used if 'velocity' and " "'position' interfaces are present"); } - if (contains(interface_types, "effort") && interface_types.size() > 1) + if ( + rsl::contains>(interface_types, "effort") && + interface_types.size() > 1) { - return ERROR("'effort' command interface has to be used alone"); + return tl::make_unexpected("'effort' command interface has to be used alone"); } - return OK; + return {}; } -Result state_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected state_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -65,25 +74,25 @@ Result state_interface_type_combinations(rclcpp::Parameter const & parameter) // 1. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' state interface cannot be used if 'position' interface " "is missing."); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "position") || - !contains(interface_types, "velocity"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' state interface cannot be used if 'position' and 'velocity' " "interfaces are not present."); } - return OK; + return {}; } } // namespace parameter_traits diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3e806ce59e..9750adb1b3 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -22,11 +22,13 @@ controller_interface control_msgs control_toolbox + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle + rsl + tl_expected trajectory_msgs - generate_parameter_library ament_cmake_gmock controller_manager From d18a10cd47ea08f3cff626224ad55f6c0c3adc40 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 12 Dec 2022 18:21:10 +0000 Subject: [PATCH 184/524] Don't run humble workflows from master branch (#479) * Remove workflows for distros not supported on master * Revert "Remove workflows for distros not supported on master" This reverts commit e916af3b19ba8642312692328ebc71a2f92ac93c. * Don't run humble workflows on master --- .github/workflows/humble-abi-compatibility.yml | 4 ++-- .github/workflows/humble-binary-build-main.yml | 8 ++++---- .github/workflows/humble-binary-build-testing.yml | 8 ++++---- .github/workflows/humble-rhel-binary-build.yml | 6 +++--- .github/workflows/humble-semi-binary-build-main.yml | 8 ++++---- .github/workflows/humble-semi-binary-build-testing.yml | 8 ++++---- .github/workflows/humble-source-build.yml | 6 +++--- 7 files changed, 24 insertions(+), 24 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 1be00cfc76..3f38d5b6ce 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -2,10 +2,10 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble jobs: abi_check: diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 01708cf864..64d78f281a 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -5,13 +5,13 @@ name: Humble Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: humble ros_repo: main upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 73ed0a4859..524cacd685 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -5,13 +5,13 @@ name: Humble Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: humble ros_repo: testing upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 8f4a65a9b5..2a4b627d5e 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -2,13 +2,13 @@ name: Humble RHEL Binary Build on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 10b1186b79..863df79a22 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -4,13 +4,13 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: main upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + 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 index ec73cc6b98..6286636e1f 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -4,13 +4,13 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: testing upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 18c33b6d52..ff0fd62e05 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -2,10 +2,10 @@ name: Humble Source Build on: workflow_dispatch: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,5 +15,5 @@ jobs: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: ros_distro: humble - ref: master + ref: humble ros2_repo_branch: humble From 42f6c1470508ca2480b954eead58a87e7e827434 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 13 Dec 2022 15:36:57 +0100 Subject: [PATCH 185/524] Fix markup in userdoc.rst (#480) --- joint_trajectory_controller/doc/userdoc.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index d225624096..8230d7febb 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -42,8 +42,9 @@ References States ^^^^^^^ The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. Legal combinations of state interfaces are: + - ``position`` - ``position`` and ``velocity`` - ``position``, ``velocity`` and ``acceleration`` From f6c4769eae0389cad62596291f88ec041558f02c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 6 Jan 2023 16:53:25 +0000 Subject: [PATCH 186/524] Add backward_ros to all controllers (#489) --- admittance_controller/CMakeLists.txt | 1 + admittance_controller/package.xml | 1 + diff_drive_controller/CMakeLists.txt | 1 + diff_drive_controller/package.xml | 1 + effort_controllers/CMakeLists.txt | 1 + effort_controllers/package.xml | 1 + force_torque_sensor_broadcaster/CMakeLists.txt | 1 + force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/CMakeLists.txt | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/CMakeLists.txt | 1 + gripper_controllers/package.xml | 1 + imu_sensor_broadcaster/CMakeLists.txt | 1 + imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/CMakeLists.txt | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/CMakeLists.txt | 1 + joint_trajectory_controller/package.xml | 1 + position_controllers/CMakeLists.txt | 1 + position_controllers/package.xml | 1 + tricycle_controller/CMakeLists.txt | 1 + tricycle_controller/package.xml | 1 + velocity_controllers/CMakeLists.txt | 1 + velocity_controllers/package.xml | 1 + 24 files changed, 24 insertions(+) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a6dcf35dce..3f121d9c3c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -34,6 +34,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 876ed424b0..69180a290f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 83f18a9931..7188f09f18 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8b2e368824..4feec7fe37 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake generate_parameter_library + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 91632606bf..d0e92dda9a 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 7476889b73..0ca0d8dffb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3aa57d4aad..eac3fe9d8b 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8cee3ef6a3..174499c573 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index ef8581d195..5dacdf66c1 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7853d78542..f6ded4ff93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 97ccd2788d..e1b32ac2b3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index b033e99b12..2734c54933 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -14,6 +14,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index e49fb65ad3..ef9d5e14e4 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 79eb5e14a7..40836967c3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 0717577e15..ee46d8eaab 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -12,6 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(generate_parameter_library REQUIRED) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c24d274a93..9fd5942d5d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -18,6 +18,7 @@ pluginlib rcutils + backward_ros control_msgs controller_interface generate_parameter_library diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index f3f8c2ab7b..af0bdb5109 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -28,6 +28,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9750adb1b3..f64bdf635e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -19,6 +19,7 @@ pluginlib realtime_tools + backward_ros controller_interface control_msgs control_toolbox diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index f1f57c6180..faf32a0e0b 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 21cdd59b86..086afccc1f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index c499bca983..e9342e8696 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 1681367c25..3c853872e9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 5f0560659a..4d7ad07dbd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 131bcd11f0..03f1bed657 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp From a49a87da1df40851a80507d61931d12c969cb015 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 18:52:18 +0100 Subject: [PATCH 187/524] [JTC] Allow ff_velocity_scale=0 without deprecated warning (#490) * Allow ff_velocity_scale=0 without deprecated warning * Remove deprecated warning for old ff_velocity_scale param --- .../src/joint_trajectory_controller.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 96110adcb2..8df4193361 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -563,21 +563,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - // TODO(destogl): remove this in ROS2 Iron - // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) - { - RCLCPP_WARN( - get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); - } - else - { - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } + ff_velocity_scale_[i] = gains.ff_velocity_scale; } } From 3829d33f236932890548b25df2812c3eb5a589a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 10 Jan 2023 22:00:35 +0100 Subject: [PATCH 188/524] =?UTF-8?q?=F0=9F=94=A7=20Fixes=20and=20updated=20?= =?UTF-8?q?on=20pre-commit=20hooks=20and=20their=20action=20(#492)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use pydocstyle instead of pep257 because it replaces it. Bump versions of hooks. * Remove fixed python version from formatting action. Fix action warnings about Node.js 12 by updating them. Bump version of clang-format. * Fixup formatting. * Bump clang-format version. * Update .pre-commit-config.yaml --- .github/workflows/ci-format.yml | 8 +++---- .pre-commit-config.yaml | 22 +++++++++---------- .../resource/joint_trajectory_controller.ui | 2 +- .../update_combo.py | 3 ++- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dd78cca0ed..b54c76d3e5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,11 +12,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v4.4.0 with: - python-version: 3.9 + python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-12 cppcheck - - uses: pre-commit/action@v2.0.3 + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..b927aeac78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,26 +33,26 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.37.3 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 22.12.0 hooks: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.2.2 hooks: - - id: pep257 + - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 5.0.4 + rev: 6.0.0 hooks: - id: flake8 args: ["--ignore=E501"] @@ -63,7 +63,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-12 + 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'] @@ -119,7 +119,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.0.0 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -136,7 +136,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist'] diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui index 3f3b6c1186..bc43f202e8 100644 --- a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -31,7 +31,7 @@ - <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namespace.</p></body></html> controller manager ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 4702d7fbc5..f0b632322f 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -48,7 +48,8 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """Check is arrays are permutation of each other. + """ + Check is arrays are permutation of each other. @type a [] first array with values to compare with the second one. @type b [] second array with values to compare with the first one. From 97748d796bfe32123444b5cb73e97c60356b9f92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 23:00:07 +0100 Subject: [PATCH 189/524] [JTC] Activate test for only velocity controller (#487) --- .../test/test_trajectory_controller.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6b0020c294..216c574228 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -421,9 +421,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->update( rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + // otherwise this won't be updated + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } // state = traj_controller_->get_node()->configure(); // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -1367,16 +1371,15 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// // only velocity controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity", "acceleration"})))); +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); // // only effort controller // INSTANTIATE_TEST_SUITE_P( From e0a373ab09cdf8c268bc3c8d14c4a3c0db59b724 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 23:01:59 +0100 Subject: [PATCH 190/524] [JTC] Add pid gain structure to documentation (#485) * Add pid gain structure to documentation * Remove trailing whitespace * Remove trailing colons --- joint_trajectory_controller/doc/userdoc.rst | 55 +++++++++++++++++---- 1 file changed, 45 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 8230d7febb..5cfbb40852 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -105,37 +105,37 @@ A yaml file for using it could be: Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joints (list(string)): +joints (list(string)) Joint names to control and listen to. -command_joints (list(string)): +command_joints (list(string)) Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. -command_interface (list(string)): +command_interface (list(string)) Command interfaces provided by the hardware interface for all joints. Values: [position | velocity | acceleration] (multiple allowed) -state_interfaces (list(string)): +state_interfaces (list(string)) State interfaces provided by the hardware for all joints. Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. -state_publish_rate (double): +state_publish_rate (double) Publish-rate of the controller's "state" topic. Default: 50.0 -action_monitor_rate (double): +action_monitor_rate (double) Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). Default: 20.0 -allow_partial_joints_goal (boolean): +allow_partial_joints_goal (boolean) Allow joint goals defining trajectory for only some joints. -open_loop_control (boolean): +open_loop_control (boolean) Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. @@ -153,16 +153,51 @@ constraints.goal_time (double) Default: 0.0 (not checked) -constraints..trajectory +constraints..trajectory (double) Maximally allowed deviation from the target trajectory for a given joint. Default: 0.0 (tolerance is not enforced) -constraints..goal +constraints..goal (double) Maximally allowed deviation from the goal (end of the trajectory) for a given joint. Default: 0.0 (tolerance is not enforced) +gains (structure) + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. This structure contains the controller gains for every joint with the control law + + .. math:: + + u = k_ff v_d + k_p (s_d - s) + k_i \sum(s_d - s) dt + k_d (v_d - v) + + with the desired velocity :math:`v_d` and position :math:`s_d`, + the measured velocity :math:`v` and position :math:`s`, the controller period :math:`dt`, + and the ``velocity`` or ``effort`` setpoint :math:`u`, respectively. + +gains..p (double) + Proportional gain :math:`k_p` for PID + + Default: 0.0 + +gains..i (double) + Integral gain :math:`k_i` for PID + + Default: 0.0 + +gains..d (double) + Derivative gain :math:`k_d` for PID + + Default: 0.0 + +gains..i_clamp (double) + Integral clamp. Symmetrical in both positive and negative direction. + + Default: 0.0 + +gains..ff_velocity_scale (double) + Feed-forward scaling :math:`k_ff` of velocity + + Default: 0.0 ROS2 interface of the controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From acaf91837838635fc1cde8da1a23498a14f03c12 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 11 Jan 2023 18:59:32 +0100 Subject: [PATCH 191/524] diff_drive base_frame_id param (#495) changed default value from `odom` -> `base_link` --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 82ba210113..fb50f2fb50 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -46,7 +46,7 @@ diff_drive_controller: } base_frame_id: { type: string, - default_value: "odom", + default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", } pose_covariance_diagonal: { From 648b1351f220d34febbd1fcf1311cb2dd61c16b7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Jan 2023 09:12:17 +0000 Subject: [PATCH 192/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 10 ++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 80 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc7fa5f2bf..8d22d15d43 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3546af9bba..bcfc7d3573 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* diff_drive base_frame_id param (`#495 `_) + changed default value from `odom` -> `base_link` +* Add backward_ros to all controllers (`#489 `_) +* Remove compilation warnings from DiffDriveController (`#477 `_) +* Contributors: Bence Magyar, Denis Štogl, Jakub Delicat + 2.15.0 (2022-12-06) ------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6954b1f7e3..d156da8f65 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 3a37a17174..18c0aafdf6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 0dc1db853b..e76e1c41ce 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e11321aefe..a39b783eed 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * Add basic gripper controller tests (`#459 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0f26b4cbf2..5457affdd1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 68d25c05aa..5da825bc04 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bc59ab4960..0bbd1f3eab 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Add pid gain structure to documentation (`#485 `_) +* [JTC] Activate test for only velocity controller (`#487 `_) +* [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) +* Add backward_ros to all controllers (`#489 `_) +* Fix markup in userdoc.rst (`#480 `_) +* [JTC] Remove deprecation from parameters validation file. (`#476 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Denis Štogl + 2.15.0 (2022-12-06) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 115fac4e37..079f474cd9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0799e154c7..a44b6f10b0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f60cb9d72a..6195129137 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d4b07df08b..c1c59d44cf 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) +* Contributors: Denis Štogl + 2.15.0 (2022-12-06) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0739bf6446..bcae54d64d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Fix deprecation warnings when compiling (`#478 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.15.0 (2022-12-06) ------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e47e52cdac..d40a89e5aa 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- From fd6a1d9244d83fac44d1e55724cbb88c565d3147 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Jan 2023 09:13:04 +0000 Subject: [PATCH 193/524] 2.16.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8d22d15d43..5c0538708d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 69180a290f..cbd4e8fe69 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.15.0 + 2.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bcfc7d3573..1295f4627e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4feec7fe37..efbed7275f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.15.0 + 2.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d156da8f65..6649f57519 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0ca0d8dffb..8f1fec01c8 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 18c0aafdf6..a4fe25b808 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 174499c573..112f96ec64 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e76e1c41ce..d165863adf 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f6ded4ff93..612f7025b3 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a39b783eed..7aba44fa13 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2734c54933..57f4103186 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 2.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5457affdd1..a8f27c3be4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 40836967c3..311cc9d4d6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5da825bc04..51d0f23c4b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9fd5942d5d..b439073e9c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.15.0 + 2.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0bbd1f3eab..5da7cda268 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f64bdf635e..55edbb5d6b 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.15.0 + 2.16.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 079f474cd9..cd4ca184f0 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 086afccc1f..e7b47e3ed8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a44b6f10b0..1cbfbc61d2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..40bacb9edc 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.15.0 + 2.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 6195129137..7052690d65 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..2243905f88 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.15.0 + 2.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..a23d99e206 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c1c59d44cf..83bb1bd382 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..096f822988 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.15.0 + 2.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..a1d67a4c6a 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bcae54d64d..062188197e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3c853872e9..e4147fe2c5 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.15.0 + 2.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index d40a89e5aa..3eb3798c5a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03f1bed657..c746c8101a 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f4788c594026bc3ecf0d944e1fa39f3f2d40673b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 14:23:34 +0100 Subject: [PATCH 194/524] [CI] Add dependabot to automatically update actions in workflows --- .github/dependabot.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..05a48fc654 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" From 8cfa22dce5e8c6c201b0f50f1a6453bfa0c78030 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:27:32 +0000 Subject: [PATCH 195/524] Bump actions/setup-python from 4.4.0 to 4.5.0 (#499) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.4.0 to 4.5.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.4.0...v4.5.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index b54c76d3e5..9816e137c6 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.4.0 + - uses: actions/setup-python@v4.5.0 with: python-version: '3.10' - name: Install system hooks From ccc3ed2e35c2773b3d296f0ccf7c7116ecb41c51 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:27:51 +0000 Subject: [PATCH 196/524] Bump ros-tooling/setup-ros from 0.3.4 to 0.4.2 (#500) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.3.4 to 0.4.2. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.3.4...0.4.2) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index d9b4ec4494..2cf4712d1f 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.4.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index d8f77a323a..87d1c8f959 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.4.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.4.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 2ad549b0f8..3c1fb519d1 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.4.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 2d0f5b4c6a2307763b39d86576003dc7738e8a75 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:10 +0000 Subject: [PATCH 197/524] Bump codecov/codecov-action from 3.1.0 to 3.1.1 (#501) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.0 to 3.1.1. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.0...v3.1.1) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 2cf4712d1f..79e0b96ca6 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.0 + - uses: codecov/codecov-action@v3.1.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 1c6f26768c43f8224fb71fe44fbfafdfdc314543 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:38 +0000 Subject: [PATCH 198/524] Bump ros-tooling/action-ros-ci from 0.2.6 to 0.2.7 (#502) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.2.6 to 0.2.7. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.2.6...0.2.7) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .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 79e0b96ca6..82c37c746a 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.2.7 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 3c1fb519d1..d5b4efd437 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@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.2.7 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From 2f9f4ea238480d97bc123bf2b4f2c0d104962b9d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:57 +0000 Subject: [PATCH 199/524] Bump pat-s/always-upload-cache from 2.1.5 to 3.0.11 (#503) Bumps [pat-s/always-upload-cache](https://github.com/pat-s/always-upload-cache) from 2.1.5 to 3.0.11. - [Release notes](https://github.com/pat-s/always-upload-cache/releases) - [Changelog](https://github.com/pat-s/always-upload-cache/blob/main/RELEASES.md) - [Commits](https://github.com/pat-s/always-upload-cache/compare/v2.1.5...v3.0.11) --- updated-dependencies: - dependency-name: pat-s/always-upload-cache dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reusable-industrial-ci-with-cache.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 490b680e7c..bca08ce5d2 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -66,14 +66,14 @@ jobs: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.BASEDIR }}/target_ws key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} From 628fe779c12ffeb7aad46a19e99e0567658c9583 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Jan 2023 07:48:38 +0000 Subject: [PATCH 200/524] Revert "2.16.0" This reverts commit fd6a1d9244d83fac44d1e55724cbb88c565d3147. --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 5c0538708d..8d22d15d43 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index cbd4e8fe69..69180a290f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.16.0 + 2.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1295f4627e..bcfc7d3573 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index efbed7275f..4feec7fe37 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.16.0 + 2.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6649f57519..d156da8f65 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 8f1fec01c8..0ca0d8dffb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a4fe25b808..18c0aafdf6 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 112f96ec64..174499c573 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.16.0 + 2.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d165863adf..e76e1c41ce 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 612f7025b3..f6ded4ff93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 7aba44fa13..a39b783eed 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 57f4103186..2734c54933 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.16.0 + 2.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a8f27c3be4..5457affdd1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 311cc9d4d6..40836967c3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.16.0 + 2.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 51d0f23c4b..5da825bc04 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index b439073e9c..9fd5942d5d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.16.0 + 2.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5da7cda268..0bbd1f3eab 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 55edbb5d6b..f64bdf635e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.16.0 + 2.15.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 cd4ca184f0..079f474cd9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e7b47e3ed8..086afccc1f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1cbfbc61d2..a44b6f10b0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 40bacb9edc..e91679802c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.16.0 + 2.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 7052690d65..6195129137 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2243905f88..269d1da466 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.16.0 + 2.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a23d99e206..ed3ae63656 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.16.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 83bb1bd382..c1c59d44cf 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 096f822988..81e69f88fd 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.16.0 + 2.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index a1d67a4c6a..afb93527f1 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.16.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 062188197e..bcae54d64d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index e4147fe2c5..3c853872e9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.16.0 + 2.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3eb3798c5a..d40a89e5aa 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index c746c8101a..03f1bed657 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f617b28be3ff82c8882bb83a32c1ee17e3b1859c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Jan 2023 07:49:19 +0000 Subject: [PATCH 201/524] 3.0.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8d22d15d43..766cc8429e 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 69180a290f..f9794297c9 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.15.0 + 3.0.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bcfc7d3573..bc295586e5 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.0.0 (2023-01-19) +------------------ * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4feec7fe37..8617f5fb1a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.15.0 + 3.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d156da8f65..c93a755898 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0ca0d8dffb..f5d69e0d7e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 18c0aafdf6..d5f331bba6 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.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 174499c573..1050d7ca32 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.15.0 + 3.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e76e1c41ce..25265a6680 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.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f6ded4ff93..f54a0b328a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a39b783eed..d0fc61b87d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2734c54933..946347e2db 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 3.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5457affdd1..0030a214ce 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.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 40836967c3..da27be0b33 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.15.0 + 3.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5da825bc04..c4f2fc3c43 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.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9fd5942d5d..01ee249201 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.15.0 + 3.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0bbd1f3eab..c84b521cb0 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.0.0 (2023-01-19) +------------------ * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f64bdf635e..9eb02f1039 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.15.0 + 3.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 079f474cd9..57182eb6e7 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 086afccc1f..8b582130a5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a44b6f10b0..07938dcf99 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..3e9a0b53ab 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.15.0 + 3.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 6195129137..39e6646486 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.0.0 (2023-01-19) +------------------ 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..7c64aceed1 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.15.0 + 3.0.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..82a97cbef6 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.15.0", + version="3.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c1c59d44cf..d0c5983bbb 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.0.0 (2023-01-19) +------------------ * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..e4b5a70b4b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.15.0 + 3.0.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..bb54029c86 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.15.0", + version="3.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bcae54d64d..3e811b8a42 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3c853872e9..90a0967f17 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.15.0 + 3.0.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index d40a89e5aa..bd9bea87eb 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03f1bed657..0cb0cacf13 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 67e5d4d394921212aa8f00eb09a68e51ef1d581e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 20 Jan 2023 10:55:25 +0100 Subject: [PATCH 202/524] [JTC] Configurable joint positon error normalization behavior (#491) * Use the same variables for state feedback and PID loop * Make joint_error normalization configurable * Activate test for only velocity controller * Allow ff_velocity_scale=0 without deprecated warning * Add test for setpoint due to normalized position error * Update comments in test Co-authored-by: Bence Magyar --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 24 ++- ...oint_trajectory_controller_parameters.yaml | 5 + .../test/test_trajectory_controller.cpp | 204 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 17 +- 5 files changed, 242 insertions(+), 10 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 a229489422..3eff7f17b1 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 @@ -166,6 +166,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_; // 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 8df4193361..733018b924 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -picomputeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], + state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } @@ -567,6 +575,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } + // Configure joint position error normalization from ROS parameters + normalize_joint_error_.resize(dof_); + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + normalize_joint_error_[i] = gains.normalize_error; + } + if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 80aa32585b..f767b15b27 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -101,6 +101,11 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling of velocity." } + normalize_error: { + type: bool, + default_value: false, + description: "Use position error normalization to -pi to pi." + } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 216c574228..511ae0869e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -487,6 +487,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are normalized if not configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are normalized if configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // is error.positions[2] normalized? + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], + state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_GE(0.0, joint_vel_[2]); + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + EXPECT_LE(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0d523cc88d..ca58b124a5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -112,6 +112,8 @@ class TestableJointTrajectoryController bool has_effort_command_interface() { return has_effort_command_interface_; } + bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -171,7 +173,8 @@ class TrajectoryControllerTest : public ::testing::Test node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } - void SetPidParameters() + void SetPidParameters( + double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -179,24 +182,26 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_p(prefix + ".p", p_default); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + 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}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, bool use_local_parameters = true, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool normalize_error = false) { SetUpTrajectoryController(executor, use_local_parameters); // set pid parameters before configure - SetPidParameters(); + SetPidParameters(k_p, ff, normalize_error); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); From 691e010a95dd3284fff32070d535cb3ae73e14af Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 24 Jan 2023 09:04:21 +0000 Subject: [PATCH 203/524] Bump actions/upload-artifact from 3.1.0 to 3.1.2 (#508) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 3.1.0 to 3.1.2. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v3.1.0...v3.1.2) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .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 82c37c746a..04cc3953d7 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -42,7 +42,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.0 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index d5b4efd437..ceb4facde7 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -43,7 +43,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v1 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From a741d4d75031cdafbb970ff0ccf2f2f8df3faaf1 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 26 Jan 2023 08:26:17 +0100 Subject: [PATCH 204/524] add publisher topic parameter to forward_position_controller (#494) --- .../publisher_forward_position_controller.py | 6 ++---- 1 file changed, 2 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 60ddd75854..59a39164ec 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 @@ -25,14 +25,14 @@ class PublisherForwardPosition(Node): def __init__(self): super().__init__("publisher_forward_position_controller") # Declare all parameters - self.declare_parameter("controller_name", "forward_position_controller") + self.declare_parameter("publish_topic", "/position_commands") self.declare_parameter("wait_sec_between_publish", 5) self.declare_parameter("goal_names", ["pos1", "pos2"]) # Read parameters - controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value goal_names = self.get_parameter("goal_names").value + publish_topic = self.get_parameter("publish_topic").value # Read all positions from parameters self.goals = [] @@ -45,8 +45,6 @@ def __init__(self): float_goal = [float(value) for value in goal] self.goals.append(float_goal) - publish_topic = "/" + controller_name + "/" + "commands" - self.get_logger().info( f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ every {wait_sec_between_publish} s' From 7c79feb7b3e76f14899efc0e3baf8a768577c90f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 26 Jan 2023 11:34:38 +0100 Subject: [PATCH 205/524] ported the joint_trajectory_controller query_state service to ROS2 (#481) * ported the joint_trajectory_controller query_state service to ROS2 * reiterate over the implementation and simplify it * updated test for long past time and some comments on service * address PR review changes Co-authored-by: Bence Magyar --- .../joint_trajectory_controller.hpp | 7 +++ .../src/joint_trajectory_controller.cpp | 51 +++++++++++++++++++ .../test/test_trajectory.cpp | 11 ++++ 3 files changed, 69 insertions(+) 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 3eff7f17b1..45d979b152 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 @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -176,6 +177,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; + rclcpp::Service::SharedPtr query_state_srv_; + std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; @@ -256,6 +259,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + void query_state_service( + const std::shared_ptr request, + std::shared_ptr response); + private: 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 733018b924..1e0ded289b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -470,6 +470,53 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +void JointTrajectoryController::query_state_service( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto logger = get_node()->get_logger(); + // Preconditions + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); + response->success = false; + return; + } + const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.joints; + trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; + if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) + { + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + response->success = (*traj_point_active_ptr_) + ->sample( + static_cast(request->time), interpolation_method_, + state_requested, start_segment_itr, end_segment_itr); + // If the requested sample time precedes the trajectory finish time respond as failure + if (response->success) + { + if (end_segment_itr == (*traj_point_active_ptr_)->end()) + { + RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); + response->success = false; + } + } + else + { + RCLCPP_ERROR( + logger, "Requested sample time is earlier than the current trajectory start time."); + } + } + else + { + RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); + response->success = false; + } + response->position = state_requested.positions; + response->velocity = state_requested.velocities; + response->acceleration = state_requested.accelerations; +} + controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { @@ -721,6 +768,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); + query_state_srv_ = get_node()->create_service( + std::string(get_node()->get_name()) + "/query_state", + std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index fbf198300b..b52aa67a04 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -189,6 +189,17 @@ TEST(TestTrajectory, sample_trajectory_positions) ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } + + // sample long past given points for same trajectory, it should receive the same end point + // so later in the query_state_service we set it to failure + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } } TEST(TestTrajectory, interpolation_pos_vel) From ec80aba8d64a222ae482476791ce4d0040803498 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Thu, 26 Jan 2023 03:54:09 -0700 Subject: [PATCH 206/524] Changing to_chrono to use nanoseconds (#507) * Changing to_chrono to use nanoseconds Previously std::chrono::seconds, which rounds to 0 if action_monitor_period_ < 1 * Reset gripper action goal timer to match JTC impl --- .../gripper_controllers/gripper_action_controller_impl.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index e28495fd1e..629f749050 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -105,9 +105,13 @@ void GripperActionController::accepted_callback( rt_active_goal_ = rt_goal; rt_active_goal_->execute(); } + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), + action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } From ae3df62ccdba42473141fe1ebc5fb45c3923edb7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 26 Jan 2023 12:48:10 +0000 Subject: [PATCH 207/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 52 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 766cc8429e..00362994d9 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bc295586e5..90ec702ab4 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.0.0 (2023-01-19) ------------------ * diff_drive base_frame_id param (`#495 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c93a755898..397c64e569 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d5f331bba6..82c921f0ab 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.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 25265a6680..4dbb74e174 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.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d0fc61b87d..bc42d03a91 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) +* Contributors: Dan Wahl + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0030a214ce..ea09f0f4ee 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.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index c4f2fc3c43..6fdfff8463 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.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c84b521cb0..c06ce877ec 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) +* [JTC] Configurable joint positon error normalization behavior (`#491 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Bence Magyar + 3.0.0 (2023-01-19) ------------------ * [JTC] Add pid gain structure to documentation (`#485 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 57182eb6e7..5c112f681d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 07938dcf99..745b489882 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 39e6646486..8f29332391 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 +----------- +* add publisher topic parameter to forward_position_controller (`#494 `_) +* Contributors: Manuel Muth + 3.0.0 (2023-01-19) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d0c5983bbb..7966781559 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.0.0 (2023-01-19) ------------------ * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3e811b8a42..cdc8804291 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bd9bea87eb..eb75415edd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) From 5298e73ebb175e0c3d08c40ea4f75c8492d5ccb7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 26 Jan 2023 12:48:47 +0000 Subject: [PATCH 208/524] 3.1.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 00362994d9..85f54999dd 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index f9794297c9..6783a5d3cd 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.0.0 + 3.1.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 90ec702ab4..69c60f6cd6 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8617f5fb1a..baff27bfe2 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.0.0 + 3.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 397c64e569..4517ec229e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index f5d69e0d7e..56d136045d 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 82c921f0ab..a09cc78472 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1050d7ca32..28ac065ec0 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.0.0 + 3.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4dbb74e174..de39f35666 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f54a0b328a..ead0fbb536 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bc42d03a91..6c72b59184 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ * Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) * Contributors: Dan Wahl diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 946347e2db..30af5a6302 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.0.0 + 3.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ea09f0f4ee..455112d852 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index da27be0b33..6a4e025c5e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.0.0 + 3.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 6fdfff8463..529a197858 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 01ee249201..4ef019d7a1 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.0.0 + 3.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c06ce877ec..a7f1fa723d 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.1.0 (2023-01-26) +------------------ * ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) * [JTC] Configurable joint positon error normalization behavior (`#491 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Bence Magyar diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9eb02f1039..fd1eec3679 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.0.0 + 3.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5c112f681d..ed743b759c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8b582130a5..d9d2852d9e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 745b489882..a86ee0c76e 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3e9a0b53ab..22951780d2 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.0.0 + 3.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 8f29332391..1521d21aae 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.1.0 (2023-01-26) +------------------ * add publisher topic parameter to forward_position_controller (`#494 `_) * Contributors: Manuel Muth diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 7c64aceed1..e1be6359bb 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.0.0 + 3.1.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 82a97cbef6..ded28dba8f 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.0.0", + version="3.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7966781559..987db88ed8 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.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index e4b5a70b4b..6d696618c7 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.0.0 + 3.1.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index bb54029c86..48b57fe993 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.0.0", + version="3.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cdc8804291..122763475b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 90a0967f17..9db6233bc8 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.0.0 + 3.1.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index eb75415edd..64a674146b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 0cb0cacf13..d47cfbad59 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 6594e72b8768b01f07b5bc7518ccf6e55d17dea6 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 31 Jan 2023 07:51:48 +0000 Subject: [PATCH 209/524] Bump ros-tooling/setup-ros from 0.4.2 to 0.5.0 (#513) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.4.2 to 0.5.0. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.4.2...0.5.0) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 04cc3953d7..ada62cd1c3 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 87d1c8f959..e5534056b8 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index ceb4facde7..f3e258a2e5 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From d9577d1d753971c4786d9a8d3edbeb9c83ee80cf Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 7 Feb 2023 17:39:33 +0100 Subject: [PATCH 210/524] Remove compile warnings. (#519) --- diff_drive_controller/src/diff_drive_controller.cpp | 4 ++-- joint_trajectory_controller/src/trajectory.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 75d8b1fa35..b6d9b07a49 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -244,7 +244,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index f28703efc9..0bdc5f7c82 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -19,8 +19,8 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rcppmath/clamp.hpp" #include "std_msgs/msg/header.hpp" + namespace joint_trajectory_controller { Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} From 762ae2b4b6012292d0f0379b215b3809deee58ae Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 7 Feb 2023 10:40:54 -0600 Subject: [PATCH 211/524] Don't set interpolation_method_ twice (#517) --- .../src/joint_trajectory_controller.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1e0ded289b..101dec8c68 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -57,9 +57,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - - // Set interpolation method from string parameter - interpolation_method_ = interpolation_methods::from_string(params_.interpolation_method); } catch (const std::exception & e) { From 6369faaa8a8bcd1bf02710370690ad9290488cfc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A1rk=20Szitanics?= Date: Wed, 8 Feb 2023 08:30:42 +0100 Subject: [PATCH 212/524] Fix JTC segfault on unload (#515) This commit fixes the segfault which happens when the JTC is unloaded, if it is in inactive state but was never activated. --- .../src/joint_trajectory_controller.cpp | 7 +++++-- .../test/test_trajectory_controller.cpp | 16 ++++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 101dec8c68..99ff99515c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -885,8 +885,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; + if (traj_home_point_ptr_ != nullptr) + { + traj_home_point_ptr_->update(traj_msg_home_ptr_); + traj_point_active_ptr_ = &traj_home_point_ptr_; + } return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 511ae0869e..8c027b4ae3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -349,6 +349,22 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // configure controller + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + // cleanup controller + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + executor.cancel(); +} + TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { rclcpp::executors::MultiThreadedExecutor executor; From 2a870b26d10e9f29e29dd02d22cbfc4f8db9b78b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 10 Feb 2023 17:14:06 +0100 Subject: [PATCH 213/524] Add JTC normalize_error parameter to doc (#511) * Add normalize_error parameter to doc * Add usecase for normalize_error * Update alignment Co-authored-by: Dr. Denis --------- Co-authored-by: Dr. Denis --- joint_trajectory_controller/doc/userdoc.rst | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 5cfbb40852..9abb84545a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -168,11 +168,10 @@ gains (structure) .. math:: - u = k_ff v_d + k_p (s_d - s) + k_i \sum(s_d - s) dt + k_d (v_d - v) + u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d` and position :math:`s_d`, - the measured velocity :math:`v` and position :math:`s`, the controller period :math:`dt`, - and the ``velocity`` or ``effort`` setpoint :math:`u`, respectively. + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. gains..p (double) Proportional gain :math:`k_p` for PID @@ -195,10 +194,18 @@ gains..i_clamp (double) Default: 0.0 gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_ff` of velocity + Feed-forward scaling :math:`k_{ff}` of velocity Default: 0.0 +gains..normalize_error (bool) + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + 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. Use this for revolute joints without end stop, + where the shortest rotation to the target position is the desired motion. + + Default: false + ROS2 interface of the controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 2ce1f21c4864bcea2707d596fe59d1f814fadf04 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 10 Feb 2023 09:24:42 -0700 Subject: [PATCH 214/524] Fix overriding of install (#510) * Fix overriding of install Signed-off-by: Tyler Weaver * Update admittance_controller/CMakeLists.txt Co-authored-by: Chris Thrasher * Update ros2_controllers/CMakeLists.txt Co-authored-by: Chris Thrasher * Remove unused lists * Use THIS_PACKAGE_INCLUDE_DEPENDS list --------- Signed-off-by: Tyler Weaver Co-authored-by: Chris Thrasher --- admittance_controller/CMakeLists.txt | 89 +++++++------- admittance_controller/package.xml | 2 - diff_drive_controller/CMakeLists.txt | 90 ++++++--------- diff_drive_controller/package.xml | 3 +- effort_controllers/CMakeLists.txt | 77 +++++-------- effort_controllers/package.xml | 3 +- .../CMakeLists.txt | 97 +++++++--------- force_torque_sensor_broadcaster/package.xml | 1 - forward_command_controller/CMakeLists.txt | 99 ++++++++-------- forward_command_controller/package.xml | 3 +- gripper_controllers/CMakeLists.txt | 52 ++++----- gripper_controllers/package.xml | 3 +- imu_sensor_broadcaster/CMakeLists.txt | 67 +++++------ imu_sensor_broadcaster/package.xml | 1 - joint_state_broadcaster/CMakeLists.txt | 108 ++++++++--------- joint_state_broadcaster/package.xml | 11 +- joint_trajectory_controller/CMakeLists.txt | 109 ++++++++---------- position_controllers/CMakeLists.txt | 77 +++++-------- position_controllers/package.xml | 3 +- ros2_controllers/CMakeLists.txt | 4 +- tricycle_controller/CMakeLists.txt | 102 +++++++--------- tricycle_controller/package.xml | 8 +- velocity_controllers/CMakeLists.txt | 80 ++++++------- velocity_controllers/package.xml | 3 +- 24 files changed, 465 insertions(+), 627 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 3f121d9c3c..9ece5e7fb0 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,26 +1,21 @@ -cmake_minimum_required(VERSION 3.5) -project(admittance_controller) +cmake_minimum_required(VERSION 3.16) +project(admittance_controller LANGUAGES CXX) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs control_toolbox controller_interface - kinematics_interface Eigen3 generate_parameter_library geometry_msgs hardware_interface joint_trajectory_controller + kinematics_interface pluginlib rclcpp rclcpp_lifecycle @@ -39,11 +34,22 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(admittance_controller SHARED src/admittance_controller.cpp) -target_include_directories(admittance_controller PRIVATE include) -generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml) -target_link_libraries(admittance_controller admittance_controller_parameters) -ament_target_dependencies(admittance_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +generate_parameter_library(admittance_controller_parameters + src/admittance_controller_parameters.yaml +) + +add_library(admittance_controller SHARED + src/admittance_controller.cpp +) +target_compile_features(admittance_controller PUBLIC cxx_std_17) +target_include_directories(admittance_controller PUBLIC + $ + $ +) +target_link_libraries(admittance_controller PUBLIC + admittance_controller_parameters +) +ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -51,43 +57,32 @@ target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_ pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS admittance_controller admittance_controller_parameters - EXPORT export_admittance_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(control_msgs REQUIRED) find_package(controller_manager REQUIRED) - find_package(controller_interface REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) + # Dynamically loaded during test + find_package(kinematics_interface_kdl REQUIRED) + # test loading admittance controller - add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) - target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) - ament_target_dependencies( - test_load_admittance_controller + add_rostest_with_parameters_gmock(test_load_admittance_controller + test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) + ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface ros2_control_test_assets ) + # test admittance controller function - add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_admittance_controller PRIVATE include) + add_rostest_with_parameters_gmock(test_admittance_controller + test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) target_link_libraries(test_admittance_controller admittance_controller) - ament_target_dependencies( - test_admittance_controller + ament_target_dependencies(test_admittance_controller control_msgs controller_interface hardware_interface @@ -95,10 +90,18 @@ if(BUILD_TESTING) ) endif() -ament_export_targets( - export_admittance_controller HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/admittance_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +install(TARGETS admittance_controller admittance_controller_parameters + EXPORT export_admittance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_admittance_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 6783a5d3cd..f2e4e227a4 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -33,9 +33,7 @@ trajectory_msgs ament_cmake_gmock - control_msgs controller_manager - hardware_interface kinematics_interface_kdl ros2_control_test_assets diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 7188f09f18..1077982380 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -1,28 +1,23 @@ -cmake_minimum_required(VERSION 3.5) -project(diff_drive_controller) +cmake_minimum_required(VERSION 3.16) +project(diff_drive_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs ) find_package(ament_cmake REQUIRED) @@ -31,39 +26,27 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) - generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml ) -add_library(${PROJECT_NAME} SHARED +add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${PROJECT_NAME} - diff_drive_controller_parameters +target_compile_features(diff_drive_controller PUBLIC cxx_std_17) +target_include_directories(diff_drive_controller PUBLIC + $ + $ ) +target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -72,11 +55,9 @@ if(BUILD_TESTING) ament_add_gmock(test_diff_drive_controller test/test_diff_drive_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) - target_include_directories(test_diff_drive_controller PRIVATE include) target_link_libraries(test_diff_drive_controller - ${PROJECT_NAME} + diff_drive_controller ) - ament_target_dependencies(test_diff_drive_controller geometry_msgs hardware_interface @@ -88,25 +69,26 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_diff_drive_controller + ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp ) - target_include_directories(test_load_diff_drive_controller PRIVATE include) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets ) - endif() -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/diff_drive_controller ) -ament_export_libraries( - ${PROJECT_NAME} +install(TARGETS diff_drive_controller diff_drive_controller_parameters + EXPORT export_diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_diff_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index baff27bfe2..32dc834e4e 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -16,6 +16,7 @@ geometry_msgs hardware_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle rcpputils @@ -23,8 +24,6 @@ tf2 tf2_msgs - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index d0e92dda9a..340bb19825 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(effort_controllers) +cmake_minimum_required(VERSION 3.16) +project(effort_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,64 +17,57 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(effort_controllers SHARED src/joint_group_effort_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(effort_controllers PUBLIC cxx_std_17) +target_include_directories(effort_controllers PUBLIC + $ + $ ) +ament_target_dependencies(effort_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_effort_controller + ament_add_gmock(test_load_joint_group_effort_controller test/test_load_joint_group_effort_controller.cpp ) - target_include_directories(test_load_joint_group_effort_controller PRIVATE include) + target_link_libraries(test_load_joint_group_effort_controller + effort_controllers + ) ament_target_dependencies(test_load_joint_group_effort_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_effort_controller + ament_add_gmock(test_joint_group_effort_controller test/test_joint_group_effort_controller.cpp ) - target_include_directories(test_joint_group_effort_controller PRIVATE include) target_link_libraries(test_joint_group_effort_controller - ${PROJECT_NAME} + effort_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/effort_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS effort_controllers + EXPORT export_effort_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_effort_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 56d136045d..6c486e059f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index eac3fe9d8b..207e978c10 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(force_torque_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(force_torque_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - geometry_msgs - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -32,81 +26,68 @@ generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) -add_library( - ${PROJECT_NAME} - SHARED +add_library(force_torque_sensor_broadcaster SHARED src/force_torque_sensor_broadcaster.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies( - ${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(force_torque_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(force_torque_sensor_broadcaster PUBLIC + $ + $ ) -target_link_libraries(force_torque_sensor_broadcaster +target_link_libraries(force_torque_sensor_broadcaster PUBLIC force_torque_sensor_broadcaster_parameters ) +ament_target_dependencies(force_torque_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) -install( - TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters - EXPORT export_force_torque_sensor_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_force_torque_sensor_broadcaster + ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp ) - target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_force_torque_sensor_broadcaster + force_torque_sensor_broadcaster + ) ament_target_dependencies(test_load_force_torque_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_force_torque_sensor_broadcaster + ament_add_gmock(test_force_torque_sensor_broadcaster test/test_force_torque_sensor_broadcaster.cpp ) - target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster - ${PROJECT_NAME} + force_torque_sensor_broadcaster ) ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_force_torque_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() -ament_export_targets( - export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/force_torque_sensor_broadcaster ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + TARGETS + force_torque_sensor_broadcaster + force_torque_sensor_broadcaster_parameters + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 28ac065ec0..e2f9b11860 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -23,7 +23,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 5dacdf66c1..46189fe5ac 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(forward_command_controller) +cmake_minimum_required(VERSION 3.16) +project(forward_command_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -37,88 +31,87 @@ generate_parameter_library( src/multi_interface_forward_command_controller_parameters.yaml ) -add_library(forward_command_controller - SHARED +add_library(forward_command_controller SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(forward_command_controller PRIVATE include) -ament_target_dependencies(forward_command_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(forward_command_controller +target_compile_features(forward_command_controller PUBLIC cxx_std_17) +target_include_directories(forward_command_controller PUBLIC + $ + $ +) +target_link_libraries(forward_command_controller PUBLIC forward_command_controller_parameters multi_interface_forward_command_controller_parameters ) +ament_target_dependencies(forward_command_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - forward_command_controller - forward_command_controller_parameters - multi_interface_forward_command_controller_parameters - EXPORT export_forward_command_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include -) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_forward_command_controller + ament_add_gmock(test_load_forward_command_controller test/test_load_forward_command_controller.cpp ) - target_include_directories(test_load_forward_command_controller PRIVATE include) + target_link_libraries(test_load_forward_command_controller + forward_command_controller + ) ament_target_dependencies(test_load_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_forward_command_controller + ament_add_gmock(test_forward_command_controller test/test_forward_command_controller.cpp ) - target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller forward_command_controller ) - ament_add_gmock( - test_load_multi_interface_forward_command_controller + ament_add_gmock(test_load_multi_interface_forward_command_controller test/test_load_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_load_multi_interface_forward_command_controller PRIVATE include) - ament_target_dependencies( - test_load_multi_interface_forward_command_controller + target_link_libraries(test_load_multi_interface_forward_command_controller + forward_command_controller + ) + ament_target_dependencies(test_load_multi_interface_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_multi_interface_forward_command_controller + ament_add_gmock(test_multi_interface_forward_command_controller test/test_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller forward_command_controller ) endif() +install( + DIRECTORY include/ + DESTINATION include/forward_command_controller +) +install( + TARGETS + forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + EXPORT export_forward_command_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + ament_export_targets(export_forward_command_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index ead0fbb536..8f241e0554 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -14,13 +14,12 @@ controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index e1b32ac2b3..e8fd1e5d7e 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -1,31 +1,25 @@ -cmake_minimum_required(VERSION 3.5) -project(gripper_controllers) +cmake_minimum_required(VERSION 3.16) +project(gripper_controllers LANGUAGES CXX) if(APPLE OR WIN32) message(WARNING "gripper controllers are not available on OSX or Windows") return() endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -41,14 +35,16 @@ generate_parameter_library(gripper_action_controller_parameters add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) -ament_target_dependencies(gripper_action_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_include_directories(gripper_action_controller - PRIVATE $ - PRIVATE $ +target_compile_features(gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(gripper_action_controller PUBLIC + $ + $ ) -target_link_libraries(gripper_action_controller +target_link_libraries(gripper_action_controller PUBLIC gripper_action_controller_parameters ) +ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) @@ -56,8 +52,7 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_gripper_action_controllers + ament_add_gmock(test_load_gripper_action_controllers test/test_load_gripper_action_controllers.cpp ) ament_target_dependencies(test_load_gripper_action_controllers @@ -65,11 +60,9 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock( - test_gripper_controllers + ament_add_gmock(test_gripper_controllers test/test_gripper_controllers.cpp ) - target_include_directories(test_gripper_controllers PRIVATE include) target_link_libraries(test_gripper_controllers gripper_action_controller ) @@ -77,9 +70,8 @@ endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/gripper_action_controller ) - install( TARGETS gripper_action_controller diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 30af5a6302..c73a85859e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -20,12 +20,11 @@ controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_action realtime_tools - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index ef9d5e14e4..18d883503b 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(imu_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(imu_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - sensor_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs ) find_package(ament_cmake REQUIRED) @@ -32,21 +26,22 @@ generate_parameter_library(imu_sensor_broadcaster_parameters src/imu_sensor_broadcaster_parameters.yaml ) -add_library( - imu_sensor_broadcaster SHARED +add_library(imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_include_directories(imu_sensor_broadcaster - PRIVATE $ - PRIVATE $ +target_compile_features(imu_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(imu_sensor_broadcaster PUBLIC + $ + $ +) +target_link_libraries(imu_sensor_broadcaster PUBLIC + imu_sensor_broadcaster_parameters ) -ament_target_dependencies(imu_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(imu_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") -target_link_libraries(imu_sensor_broadcaster - imu_sensor_broadcaster_parameters -) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) @@ -54,43 +49,35 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_imu_sensor_broadcaster + ament_add_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp ) - target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_imu_sensor_broadcaster + imu_sensor_broadcaster + ) ament_target_dependencies(test_load_imu_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_imu_sensor_broadcaster + ament_add_gmock(test_imu_sensor_broadcaster test/test_imu_sensor_broadcaster.cpp ) - target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster ) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_imu_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/imu_sensor_broadcaster ) - install( TARGETS imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6a4e025c5e..10cd44ccfa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -25,7 +25,6 @@ ament_lint_auto ament_lint_common controller_manager - hardware_interface ros2_control_test_assets diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index ee46d8eaab..018adef23a 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -1,37 +1,11 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_state_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(joint_state_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(control_msgs REQUIRED) -find_package(controller_interface REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rcutils REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(sensor_msgs REQUIRED) - -generate_parameter_library(joint_state_broadcaster_parameters - src/joint_state_broadcaster_parameters.yaml -) - -add_library(joint_state_broadcaster - SHARED - src/joint_state_broadcaster.cpp -) -target_include_directories(joint_state_broadcaster PRIVATE include) -ament_target_dependencies(joint_state_broadcaster +set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces control_msgs controller_interface @@ -42,27 +16,34 @@ ament_target_dependencies(joint_state_broadcaster realtime_tools sensor_msgs ) -target_link_libraries(joint_state_broadcaster + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(joint_state_broadcaster_parameters + src/joint_state_broadcaster_parameters.yaml +) + +add_library(joint_state_broadcaster SHARED + src/joint_state_broadcaster.cpp +) +target_compile_features(joint_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(joint_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(joint_state_broadcaster PUBLIC joint_state_broadcaster_parameters ) +ament_target_dependencies(joint_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - joint_state_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -70,38 +51,43 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_state_broadcaster + ament_add_gmock(test_load_joint_state_broadcaster test/test_load_joint_state_broadcaster.cpp ) - target_include_directories(test_load_joint_state_broadcaster PRIVATE include) + target_link_libraries(test_load_joint_state_broadcaster + joint_state_broadcaster + ) ament_target_dependencies(test_load_joint_state_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_joint_state_broadcaster + ament_add_gmock(test_joint_state_broadcaster test/test_joint_state_broadcaster.cpp ) - target_include_directories(test_joint_state_broadcaster PRIVATE include) target_link_libraries(test_joint_state_broadcaster joint_state_broadcaster ) + ament_target_dependencies(test_joint_state_broadcaster + hardware_interface + ) endif() -ament_export_dependencies( - control_msgs - controller_interface - generate_parameter_library - rclcpp_lifecycle - sensor_msgs -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/joint_state_broadcaster ) -ament_export_libraries( - joint_state_broadcaster +install( + TARGETS + joint_state_broadcaster + joint_state_broadcaster_parameters + EXPORT export_joint_state_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 4ef019d7a1..2de9e16b63 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -12,23 +12,20 @@ ament_cmake - pluginlib - rcutils - - pluginlib - rcutils - backward_ros + builtin_interfaces control_msgs controller_interface generate_parameter_library - hardware_interface + pluginlib rclcpp_lifecycle + rcutils realtime_tools sensor_msgs ament_cmake_gmock controller_manager + hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index af0bdb5109..5ec478d989 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -1,30 +1,24 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_trajectory_controller) +cmake_minimum_required(VERSION 3.16) +project(joint_trajectory_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - angles - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - rsl - tl_expected - trajectory_msgs + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rsl + tl_expected + trajectory_msgs ) find_package(ament_cmake REQUIRED) @@ -38,55 +32,47 @@ generate_parameter_library(joint_trajectory_controller_parameters include/joint_trajectory_controller/validate_jtc_parameters.hpp ) -add_library(${PROJECT_NAME} SHARED +add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -target_link_libraries(${PROJECT_NAME} joint_trajectory_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) +target_include_directories(joint_trajectory_controller PUBLIC + $ + $ +) +target_link_libraries(joint_trajectory_controller PUBLIC + joint_trajectory_controller_parameters +) +ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters - EXPORT export_joint_trajectory_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_trajectory test/test_trajectory.cpp) - target_include_directories(test_trajectory PRIVATE include) - target_link_libraries(test_trajectory ${PROJECT_NAME}) + target_link_libraries(test_trajectory joint_trajectory_controller) ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) - target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller - ${PROJECT_NAME} - ) - ament_target_dependencies(test_trajectory_controller - ${THIS_PACKAGE_INCLUDE_DEPENDS} + joint_trajectory_controller ) - ament_add_gmock( - test_load_joint_trajectory_controller + ament_add_gmock(test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp ) - target_include_directories(test_load_joint_trajectory_controller PRIVATE include) + target_link_libraries(test_load_joint_trajectory_controller + joint_trajectory_controller + ) ament_target_dependencies(test_load_joint_trajectory_controller controller_manager control_toolbox @@ -95,23 +81,28 @@ if(BUILD_TESTING) ) # TODO(andyz): Disabled due to flakiness - # ament_add_gmock( - # test_trajectory_actions + # ament_add_gmock(test_trajectory_actions # test/test_trajectory_actions.cpp # ) - # target_include_directories(test_trajectory_actions PRIVATE include) # target_link_libraries(test_trajectory_actions - # ${PROJECT_NAME} - # ) - # ament_target_dependencies(test_trajectory_actions - # ${THIS_PACKAGE_INCLUDE_DEPENDS} + # joint_trajectory_controller # ) endif() -ament_export_targets( - export_joint_trajectory_controller HAS_LIBRARY_TARGET + +install( + DIRECTORY include/ + DESTINATION include/joint_trajectory_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install(TARGETS + joint_trajectory_controller + joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index faf32a0e0b..05714a6e46 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(position_controllers) +cmake_minimum_required(VERSION 3.16) +project(position_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,64 +17,57 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(position_controllers SHARED src/joint_group_position_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(position_controllers PUBLIC cxx_std_17) +target_include_directories(position_controllers PUBLIC + $ + $ ) +ament_target_dependencies(position_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_position_controller + ament_add_gmock(test_load_joint_group_position_controller test/test_load_joint_group_position_controller.cpp ) - target_include_directories(test_load_joint_group_position_controller PRIVATE include) + target_link_libraries(test_load_joint_group_position_controller + position_controllers + ) ament_target_dependencies(test_load_joint_group_position_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_position_controller + ament_add_gmock(test_joint_group_position_controller test/test_joint_group_position_controller.cpp ) - target_include_directories(test_joint_group_position_controller PRIVATE include) target_link_libraries(test_joint_group_position_controller - ${PROJECT_NAME} + position_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/position_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS position_controllers + EXPORT export_position_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_position_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/position_controllers/package.xml b/position_controllers/package.xml index d9d2852d9e..a624b73066 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/ros2_controllers/CMakeLists.txt b/ros2_controllers/CMakeLists.txt index 8e3b75944d..bb7e2d25bb 100644 --- a/ros2_controllers/CMakeLists.txt +++ b/ros2_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.5) -project(ros2_controllers) +cmake_minimum_required(VERSION 3.16) +project(ros2_controllers LANGUAGES NONE) find_package(ament_cmake REQUIRED) ament_package() diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index e9342e8696..ea2232f88f 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -1,71 +1,47 @@ -cmake_minimum_required(VERSION 3.8) -project(tricycle_controller) +cmake_minimum_required(VERSION 3.16) +project(tricycle_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(ackermann_msgs REQUIRED) - -add_library( - ${PROJECT_NAME} - SHARED - src/tricycle_controller.cpp - src/odometry.cpp - src/traction_limiter.cpp - src/steering_limiter.cpp -) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) - -ament_target_dependencies( - ${PROJECT_NAME} +set(THIS_PACKAGE_INCLUDE_DEPENDS ackermann_msgs builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs ) -pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +add_library(tricycle_controller SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp ) -install( - DIRECTORY - include/ - DESTINATION include +target_compile_features(tricycle_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_controller PUBLIC + $ + $ ) +ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -75,11 +51,9 @@ if(BUILD_TESTING) ament_add_gmock(test_tricycle_controller test/test_tricycle_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) - target_include_directories(test_tricycle_controller PRIVATE include) target_link_libraries(test_tricycle_controller - ${PROJECT_NAME} + tricycle_controller ) - ament_target_dependencies(test_tricycle_controller geometry_msgs hardware_interface @@ -91,29 +65,31 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_tricycle_controller + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp ) - target_include_directories(test_load_tricycle_controller PRIVATE include) + target_link_libraries(test_load_tricycle_controller + tricycle_controller + ) ament_target_dependencies(test_load_tricycle_controller controller_manager ros2_control_test_assets ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include/tricycle_controller ) -ament_export_dependencies( - controller_interface - geometry_msgs - hardware_interface - rclcpp - rclcpp_lifecycle +install( + TARGETS + tricycle_controller + EXPORT export_tricycle_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_tricycle_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9db6233bc8..f5a649ab7a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -11,21 +11,21 @@ ament_cmake + ackermann_msgs backward_ros + builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs + pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs - ackermann_msgs - - pluginlib ament_cmake_gmock controller_manager diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 4d7ad07dbd..27ec8417bb 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(velocity_controllers) +cmake_minimum_required(VERSION 3.16) +project(velocity_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,67 +17,59 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(velocity_controllers SHARED src/joint_group_velocity_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - forward_command_controller - pluginlib - rclcpp +target_compile_features(velocity_controllers PUBLIC cxx_std_17) +target_include_directories(velocity_controllers PUBLIC + $ + $ ) +ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_velocity_controller + ament_add_gmock(test_load_joint_group_velocity_controller test/test_load_joint_group_velocity_controller.cpp ) - target_include_directories(test_load_joint_group_velocity_controller PRIVATE include) + target_link_libraries(test_load_joint_group_velocity_controller + velocity_controllers + ) ament_target_dependencies(test_load_joint_group_velocity_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_velocity_controller + ament_add_gmock(test_joint_group_velocity_controller test/test_joint_group_velocity_controller.cpp ) - target_include_directories(test_joint_group_velocity_controller PRIVATE include) target_link_libraries(test_joint_group_velocity_controller - ${PROJECT_NAME} + velocity_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/velocity_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS + velocity_controllers + EXPORT export_velocity_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d47cfbad59..e48b8b331e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager hardware_interface From 7cc34343e8becfa0cd408ca2890992caa6e8262f Mon Sep 17 00:00:00 2001 From: Solomon Wiznitzer <31753673+swiz23@users.noreply.github.com> Date: Fri, 10 Feb 2023 11:38:51 -0500 Subject: [PATCH 215/524] fix interpolation logic (#516) Co-authored-by: Solomon Wiznitzer --- .../joint_trajectory_controller/interpolation_methods.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index 766be6e0f4..df8c9da0c4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -45,7 +45,7 @@ const std::unordered_map InterpolationMethodMa return InterpolationMethod::NONE; } else if ( - !interpolation_method.compare( + interpolation_method.compare( InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) { return InterpolationMethod::VARIABLE_DEGREE_SPLINE; From 3a1402e5fb3862c718a36d863d2f7cfde852cb23 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Fri, 10 Feb 2023 11:59:46 -0500 Subject: [PATCH 216/524] fix JTC segfault (#518) Co-authored-by: Michael Wiznitzer --- joint_trajectory_controller/src/trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0bdc5f7c82..06b3ca6e9b 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -64,7 +64,6 @@ bool Trajectory::sample( TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) - output_state = trajectory_msgs::msg::JointTrajectoryPoint(); if (trajectory_msg_->points.empty()) { @@ -90,6 +89,7 @@ bool Trajectory::sample( return false; } + output_state = trajectory_msgs::msg::JointTrajectoryPoint(); auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; From bebe50627427c1a87428435f3942ee01d94d23a8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 10 Feb 2023 17:38:11 +0000 Subject: [PATCH 217/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 78 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 85f54999dd..0391596f3a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 69c60f6cd6..a88f5fbd5e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Remove compile warnings. (`#519 `_) +* Contributors: Dr. Denis, Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4517ec229e..cd0f62e23a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a09cc78472..1323085416 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index de39f35666..900a4bdbcd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6c72b59184..1438138806 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ * Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 455112d852..573824d698 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 529a197858..3df34a4f74 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a7f1fa723d..1348c97891 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix JTC segfault (`#518 `_) +* fix interpolation logic (`#516 `_) +* Fix overriding of install (`#510 `_) +* Add JTC normalize_error parameter to doc (`#511 `_) +* Fix JTC segfault on unload (`#515 `_) +* Don't set interpolation_method\_ twice (`#517 `_) +* Remove compile warnings. (`#519 `_) +* Contributors: Andy Zelenak, Christoph Fröhlich, Dr. Denis, Michael Wiznitzer, Márk Szitanics, Solomon Wiznitzer, Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ * ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ed743b759c..c1e0d551ef 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a86ee0c76e..3c4d2067ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 1521d21aae..a6ed0ae7cd 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.1.0 (2023-01-26) ------------------ * add publisher topic parameter to forward_position_controller (`#494 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 987db88ed8..e36f0590db 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.1.0 (2023-01-26) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 122763475b..680c90331b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 64a674146b..fd114dbb25 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ From d8c2946a89f7d7218bf4c36436e7364517975345 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 10 Feb 2023 17:38:49 +0000 Subject: [PATCH 218/524] 3.2.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0391596f3a..f06aeb1811 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index f2e4e227a4..63101a6fca 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.1.0 + 3.2.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a88f5fbd5e..e783350de9 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.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Remove compile warnings. (`#519 `_) * Contributors: Dr. Denis, Tyler Weaver, Chris Thrasher diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 32dc834e4e..869ee842b1 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.1.0 + 3.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index cd0f62e23a..b689368ba0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6c486e059f..685c71638f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 1323085416..21063cf757 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.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index e2f9b11860..b80ef06beb 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.1.0 + 3.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 900a4bdbcd..28b949f0f4 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.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8f241e0554..c1c1f8b03d 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 1438138806..8b14f7d03f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index c73a85859e..28ff8e3045 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.1.0 + 3.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 573824d698..52cd4bedbc 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.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 10cd44ccfa..55a196b0cf 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.1.0 + 3.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3df34a4f74..7ff57c523c 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.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 2de9e16b63..1cf8db7390 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.1.0 + 3.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1348c97891..ee749061b4 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.2.0 (2023-02-10) +------------------ * fix JTC segfault (`#518 `_) * fix interpolation logic (`#516 `_) * Fix overriding of install (`#510 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fd1eec3679..77042cff45 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.1.0 + 3.2.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 c1e0d551ef..e96b0bd8e2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/position_controllers/package.xml b/position_controllers/package.xml index a624b73066..c574571958 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 3c4d2067ca..6ea4b96eae 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 22951780d2..89c958d363 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.1.0 + 3.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a6ed0ae7cd..8b6a0ea2ac 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.2.0 (2023-02-10) +------------------ 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index e1be6359bb..9de7202716 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.1.0 + 3.2.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ded28dba8f..71733ca335 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.1.0", + version="3.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index e36f0590db..a5b01a0360 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.2.0 (2023-02-10) +------------------ 3.1.0 (2023-01-26) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 6d696618c7..f0189c15ca 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.1.0 + 3.2.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 48b57fe993..8673f708e4 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.1.0", + version="3.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 680c90331b..3d58adaa84 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index f5a649ab7a..c34e7ef743 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.1.0 + 3.2.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index fd114dbb25..42c0eccc77 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e48b8b331e..4d03238ddf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From abe9efdb03cd6f5e68f788547a87b90ec229b24b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 18 Feb 2023 07:12:26 +0100 Subject: [PATCH 219/524] Add FTS broadcaster to list (#528) --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index dee9d31abe..5eccbd9841 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,3 +56,4 @@ Available Broadcasters Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> From 520036dac2b548e24b76ee0bd84658af36bfa7d8 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Mon, 20 Feb 2023 02:02:33 -0500 Subject: [PATCH 220/524] Fix Segfault in GripperActionController (#527) * Use RT buffer for gripper action controller's goal handle * fix format --------- Co-authored-by: Bence Magyar --- .../gripper_action_controller.hpp | 4 +- .../gripper_action_controller_impl.hpp | 57 ++++++++++--------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 830de7b514..89bf232fad 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -115,6 +115,7 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; using HwIfaceAdapter = HardwareInterfaceAdapter; @@ -134,7 +135,8 @@ class GripperActionController : public controller_interface::ControllerInterface HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 629f749050..666fced1c0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -28,11 +28,12 @@ template void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - if (rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { // Marks the current goal as canceled - rt_active_goal_->setCanceled(std::make_shared()); - rt_active_goal_.reset(); + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } @@ -86,25 +87,23 @@ template void GripperActionController::accepted_callback( std::shared_ptr goal_handle) // Try to update goal { - { - auto rt_goal = std::make_shared(goal_handle); + auto rt_goal = std::make_shared(goal_handle); - // Accept new goal - preempt_active_goal(); + // Accept new goal + preempt_active_goal(); - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_ = goal_handle->get_goal()->command.position; + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + command_.writeFromNonRT(command_struct_); - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; - last_movement_time_ = get_node()->now(); - rt_active_goal_ = rt_goal; - rt_active_goal_->execute(); - } + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -112,7 +111,7 @@ void GripperActionController::accepted_callback( // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } template @@ -122,7 +121,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode set_hold_position(); @@ -132,9 +132,9 @@ rclcpp_action::CancelResponse GripperActionController::cancel // Mark the current goal as canceled auto action_res = std::make_shared(); - rt_active_goal_->setCanceled(action_res); + active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -152,7 +152,8 @@ void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - if (!rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) { return; } @@ -164,8 +165,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); - rt_active_goal_->setSucceeded(pre_alloc_result_); - rt_active_goal_.reset(); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } else { @@ -183,14 +184,14 @@ void GripperActionController::check_for_success( if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); - rt_active_goal_->setSucceeded(pre_alloc_result_); + active_goal->setSucceeded(pre_alloc_result_); } else { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); - rt_active_goal_->setAborted(pre_alloc_result_); + active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } } From 5a7aedeae7e4c1fb76a0498112ee22bc2b8e8a1d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 20 Feb 2023 16:55:34 +0100 Subject: [PATCH 221/524] Remove publish_rate argument (#529) It was removed: https://github.com/ros-controls/ros2_controllers/pull/468 --- tricycle_controller/config/tricycle_drive_controller.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index c1077b13c3..fba6f34bb2 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -9,7 +9,6 @@ tricycle_controller: # Odometry odom_frame_id: odom base_frame_id: base_link - publish_rate: 50.0 # publish rate of odom and tf open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry enable_odom_tf: true # If True, publishes odom<-base_link TF odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf From 9c53e68c2bc4c35477f634c9e056e610da592d34 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 22 Feb 2023 15:26:05 +0100 Subject: [PATCH 222/524] =?UTF-8?q?=F0=9F=95=B0=EF=B8=8F=20remove=20state?= =?UTF-8?q?=20publish=20rate=20from=20JTC.=20(#520)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../joint_trajectory_controller.hpp | 7 +-- .../src/joint_trajectory_controller.cpp | 35 ++------------ ...oint_trajectory_controller_parameters.yaml | 8 ---- .../test/test_trajectory_controller.cpp | 47 ------------------- .../test/test_trajectory_controller_utils.hpp | 3 -- 5 files changed, 7 insertions(+), 93 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 45d979b152..c8c94aa95d 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 @@ -192,9 +192,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; - rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); - rclcpp::Time last_state_publish_time_; - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; @@ -252,8 +249,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error); + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); void read_state_from_hardware(JointTrajectoryPoint & state); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 99ff99515c..bb26a9bb46 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -356,7 +356,7 @@ controller_interface::return_type JointTrajectoryController::update( } } - publish_state(state_desired_, state_current_, state_error_); + publish_state(time, state_desired_, state_current_, state_error_); return controller_interface::return_type::OK; } @@ -705,17 +705,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); - // State publisher - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", params_.state_publish_rate); - if (params_.state_publish_rate > 0.0) - { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / params_.state_publish_rate); - } - else - { - state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); - } - publisher_ = get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); @@ -739,8 +728,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } state_publisher_->unlock(); - last_state_publish_time_ = get_node()->now(); - // action server configuration if (params_.allow_partial_joints_goal) { @@ -826,7 +813,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset read_state_from_hardware(state_current_); @@ -936,23 +922,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( } void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error) + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { - if (state_publisher_period_.seconds() <= 0.0) - { - return; - } - - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) - { - return; - } - - if (state_publisher_ && state_publisher_->trylock()) + if (state_publisher_->trylock()) { - last_state_publish_time_ = get_node()->now(); - state_publisher_->msg_.header.stamp = last_state_publish_time_; + state_publisher_->msg_.header.stamp = time; state_publisher_->msg_.desired.positions = desired_state.positions; state_publisher_->msg_.desired.velocities = desired_state.velocities; state_publisher_->msg_.desired.accelerations = desired_state.accelerations; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index f767b15b27..2e0d2018f8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -50,14 +50,6 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } - state_publish_rate: { - type: double, - default_value: 50.0, - description: "Rate controller state is published", - validation: { - lower_bounds: [0.1] - } - } action_monitor_rate: { type: double, default_value: 20.0, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8c027b4ae3..1c0dbb5482 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -707,53 +707,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) executor.cancel(); } -void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -{ - rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", static_cast(target_msg_count)); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); - - auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); - - using control_msgs::msg::JointTrajectoryControllerState; - - const int qos_level = 10; - int echo_received_counter = 0; - rclcpp::Subscription::SharedPtr subs = - traj_controller_->get_node()->create_subscription( - controller_name_ + "/state", qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); - - // update for 1second - auto clock = rclcpp::Clock(RCL_STEADY_TIME); - const auto start_time = clock.now(); - const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); - const auto end_time = start_time + wait; - while (clock.now() < end_time) - { - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - } - - // We may miss the last message since time allowed is exactly the time needed - EXPECT_NEAR(target_msg_count, echo_received_counter, 1); - - executor.cancel(); -} - -/** - * @brief test_state_publish_rate Test that state publish rate matches configure rate - */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -{ - test_state_publish_rate_target(10); -} - -// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -// { -// test_state_publish_rate_target(0); -// } - // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from // * internal controller order diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index ca58b124a5..25612f56e9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -273,8 +273,6 @@ class TrajectoryControllerTest : public ::testing::Test void subscribeToState() { auto traj_lifecycle_node = traj_controller_->get_node(); - traj_lifecycle_node->set_parameter( - rclcpp::Parameter("state_publish_rate", static_cast(100))); using control_msgs::msg::JointTrajectoryControllerState; @@ -410,7 +408,6 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); return state_msg_; } - void test_state_publish_rate_target(int target_msg_count); std::string controller_name_; From bc5d285b6cd56668371eba90e718de0be0bfc79a Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:57:03 +0000 Subject: [PATCH 223/524] Bump ros-tooling/setup-ros from 0.5.0 to 0.6.0 (#534) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.5.0 to 0.6.0. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.5.0...0.6.0) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index ada62cd1c3..9b6723e1c9 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index e5534056b8..17fa2a7f25 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f3e258a2e5..57ef0dd0e6 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 5e943b6f88d23174f388deccc2ef276bb7764085 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:57:49 +0000 Subject: [PATCH 224/524] Bump ros-tooling/action-ros-ci from 0.2.7 to 0.3.0 (#535) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.2.7 to 0.3.0. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.2.7...0.3.0) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .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 9b6723e1c9..120376c285 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.7 + - uses: ros-tooling/action-ros-ci@0.3.0 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 57ef0dd0e6..487151a99b 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@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.7 + - uses: ros-tooling/action-ros-ci@0.3.0 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From cfaa7088f67df55d51ba9f1e2207bdb01faf3dfb Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:58:59 +0000 Subject: [PATCH 225/524] Bump actions/checkout from 1 to 3 (#536) Bumps [actions/checkout](https://github.com/actions/checkout) from 1 to 3. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v1...v3) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reviewer_lottery.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..94f3d9bde6 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: uesteibar/reviewer-lottery@v2 with: repo-token: ${{ secrets.GITHUB_TOKEN }} From 1bdc4a49f25db7188fe02f20edd473a670e7cf02 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 1 Mar 2023 09:50:37 -0600 Subject: [PATCH 226/524] Add comments about auto-generated header files (#539) --- .../include/admittance_controller/admittance_controller.hpp | 2 +- .../include/diff_drive_controller/diff_drive_controller.hpp | 1 + .../force_torque_sensor_broadcaster.hpp | 1 + .../forward_command_controller/forward_command_controller.hpp | 1 + .../include/gripper_controllers/gripper_action_controller.hpp | 4 +++- .../include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 1 + .../joint_state_broadcaster/joint_state_broadcaster.hpp | 1 + .../joint_trajectory_controller.hpp | 1 + 8 files changed, 10 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index f776646d42..976c877b6f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -22,7 +22,7 @@ #include #include -// include generated parameter library +// auto-generated by generate_parameter_library #include "admittance_controller_parameters.hpp" #include "admittance_controller/admittance_rule.hpp" 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 f229667fb9..277c7bd99b 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 @@ -42,6 +42,7 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" +// auto-generated by generate_parameter_library #include "diff_drive_controller_parameters.hpp" namespace diff_drive_controller diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 6166adfd9a..754d1de9ba 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 8a697847cd..91e3aae480 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -21,6 +21,7 @@ #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +// auto-generated by generate_parameter_library #include "forward_command_controller_parameters.hpp" namespace forward_command_controller diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 89bf232fad..9d67249405 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -41,9 +41,11 @@ #include "realtime_tools/realtime_server_goal_handle.h" // Project -#include "gripper_action_controller_parameters.hpp" #include "gripper_controllers/hardware_interface_adapter.hpp" +// auto-generated by generate_parameter_library +#include "gripper_action_controller_parameters.hpp" + namespace gripper_action_controller { /** diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index d78e28780e..cd2a44d32b 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 4761f3f250..f1c532dce9 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -23,6 +23,7 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "joint_state_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 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 c8c94aa95d..2a6454f454 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 @@ -45,6 +45,7 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +// auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" using namespace std::chrono_literals; // NOLINT From 2f01639ebed824ceebe76ae76778ad7f61dcfe58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 2 Mar 2023 22:44:12 +0100 Subject: [PATCH 227/524] Use std::clamp instead of rcppmath::clamp (#540) * Use std::clamp * Remove includes --- tricycle_controller/src/steering_limiter.cpp | 7 +++---- tricycle_controller/src/traction_limiter.cpp | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index b76cc0f602..eec4b20811 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -20,7 +20,6 @@ #include #include -#include "rcppmath/clamp.hpp" #include "tricycle_controller/steering_limiter.hpp" namespace tricycle_controller @@ -76,7 +75,7 @@ double SteeringLimiter::limit(double & p, double p0, double p1, double dt) double SteeringLimiter::limit_position(double & p) { const double tmp = p; - p = rcppmath::clamp(p, min_position_, max_position_); + p = std::clamp(p, min_position_, max_position_); return tmp != 0.0 ? p / tmp : 1.0; } @@ -88,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); + double dv = std::clamp(std::fabs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -107,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); + double da = std::clamp(std::fabs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index 7865d4be71..b9759e3624 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -20,7 +20,6 @@ #include #include -#include "rcppmath/clamp.hpp" #include "tricycle_controller/traction_limiter.hpp" namespace tricycle_controller @@ -91,7 +90,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); + v = std::clamp(std::fabs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +112,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); + double dv = std::clamp(std::fabs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +131,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); + double da = std::clamp(std::fabs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; From ee3fdd020228d63e9efaf86d2315405de1f4e5b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Mar 2023 21:03:21 +0000 Subject: [PATCH 228/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 66 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index f06aeb1811..3af07c8680 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e783350de9..db1f5da5db 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b689368ba0..5aaca59498 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 21063cf757..a9dc27dd3a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 28b949f0f4..149cda8e29 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8b14f7d03f..1416f78be8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Fix Segfault in GripperActionController (`#527 `_) +* Contributors: AndyZe, Erik Holum + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 52cd4bedbc..a682550c63 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 7ff57c523c..663546aa6d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ee749061b4..c7e4884466 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* 🕰️ remove state publish rate from JTC. (`#520 `_) +* Contributors: AndyZe, Dr. Denis + 3.2.0 (2023-02-10) ------------------ * fix JTC segfault (`#518 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e96b0bd8e2..e567a2e196 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6ea4b96eae..cc68f9b8e8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 8b6a0ea2ac..5d9f97e0be 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index a5b01a0360..7182e5a8cc 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.2.0 (2023-02-10) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3d58adaa84..bf762f882b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use std::clamp instead of rcppmath::clamp (`#540 `_) +* Remove publish_rate argument (`#529 `_) +* Contributors: Christoph Fröhlich, Tony Najjar + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 42c0eccc77..649232a83d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) From d78425359fcc3d3f16fbe7f9302d5fbc673ef4c1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Mar 2023 21:03:47 +0000 Subject: [PATCH 229/524] 3.3.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3af07c8680..f0ffaa5f41 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 63101a6fca..1b7a90e7e7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.2.0 + 3.3.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index db1f5da5db..1c20abd5d8 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 869ee842b1..16973cf821 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.2.0 + 3.3.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 5aaca59498..e38c07b589 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 685c71638f..c72b974644 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.2.0 + 3.3.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 a9dc27dd3a..a0b8cf5bd0 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index b80ef06beb..fd6a0de5bf 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.2.0 + 3.3.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 149cda8e29..d87577328b 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c1c1f8b03d..388b931a93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 1416f78be8..9291bb6098 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Fix Segfault in GripperActionController (`#527 `_) * Contributors: AndyZe, Erik Holum diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 28ff8e3045..cc61c5d105 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.2.0 + 3.3.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a682550c63..a2bd1899f5 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 55a196b0cf..aa5d4ffa6e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.2.0 + 3.3.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 663546aa6d..8502816fd7 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1cf8db7390..e8af283875 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.2.0 + 3.3.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 c7e4884466..5ab2f01fcd 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.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * 🕰️ remove state publish rate from JTC. (`#520 `_) * Contributors: AndyZe, Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 77042cff45..7c1f9c74c1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.2.0 + 3.3.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 e567a2e196..ef05e136bd 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c574571958..180348f776 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index cc68f9b8e8..8933ec9141 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 89c958d363..429efd1aa0 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.2.0 + 3.3.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 5d9f97e0be..157b9a853c 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.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 9de7202716..01a961571a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.2.0 + 3.3.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 71733ca335..a30e10b9df 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.2.0", + version="3.3.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 7182e5a8cc..8c44f8ab1d 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.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index f0189c15ca..9ffac65a15 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.2.0 + 3.3.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 8673f708e4..62259dcf39 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.2.0", + version="3.3.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bf762f882b..c86256d5e5 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Use std::clamp instead of rcppmath::clamp (`#540 `_) * Remove publish_rate argument (`#529 `_) * Contributors: Christoph Fröhlich, Tony Najjar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index c34e7ef743..0c89f6dc05 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.2.0 + 3.3.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 649232a83d..11291b3b5d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 4d03238ddf..e197b29f72 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 8bbabb5193abc8a233753e949e5597b5499dad92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 7 Mar 2023 22:56:05 +0100 Subject: [PATCH 230/524] Update JTC documentation (#541) * Update JTC documentation Update, e.g. parameter changed with #520 * Fix whitespaces --- joint_trajectory_controller/doc/userdoc.rst | 22 ++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 9abb84545a..e3d5c1976a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -89,7 +89,6 @@ A yaml file for using it could be: - position - velocity - state_publish_rate: 50.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false @@ -122,11 +121,6 @@ state_interfaces (list(string)) Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. -state_publish_rate (double) - Publish-rate of the controller's "state" topic. - - Default: 50.0 - action_monitor_rate (double) Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). @@ -135,9 +129,23 @@ action_monitor_rate (double) allow_partial_joints_goal (boolean) Allow joint goals defining trajectory for only some joints. + Default: false + +allow_integration_in_goal_trajectories (boolean) + Allow integration in goal trajectories to accept goals without position or velocity specified + + Default: false + +interpolation_method (string) + The type of interpolation to use, if any. Can be "splines" or "none". + + Default: splines + open_loop_control (boolean) Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + Default: false + If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. constraints (structure) @@ -213,7 +221,7 @@ ROS2 interface of the controller Topic for commanding the controller. ~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] - Topic publishing internal states. + Topic publishing internal states with the update-rate of the controller manager. ~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller. From 2bf8ce028ec408df9f7e1114e54070215e1846ec Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Mar 2023 06:27:25 +0000 Subject: [PATCH 231/524] Bump ros-tooling/setup-ros from 0.6.0 to 0.6.1 (#544) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.6.0 to 0.6.1. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.6.0...0.6.1) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 120376c285..f7854aae5b 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 17fa2a7f25..5dd0b6f086 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 487151a99b..51519dfb2c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 82a4e2434c8f766c22c23fc11ffb38d54404c212 Mon Sep 17 00:00:00 2001 From: GuiHome Date: Thu, 16 Mar 2023 22:33:22 +0100 Subject: [PATCH 232/524] Removed auto param decl (#546) automatically_declare_parameters_from_overrides True hinders from dynamically loading external libraries declaring their own parameters. --- admittance_controller/test/test_admittance_controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index be78f05bb9..499aec7de9 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -167,7 +167,7 @@ class AdmittanceControllerTest : public ::testing::Test auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false) .parameter_overrides(parameter_overrides) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(false); return SetUpControllerCommon(controller_name, options); } @@ -176,7 +176,7 @@ class AdmittanceControllerTest : public ::testing::Test { auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(false); return SetUpControllerCommon(controller_name, options); } From eb76cd75aa4b902be309d97ff90ba7e69b39cf01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 Mar 2023 20:45:11 +0200 Subject: [PATCH 233/524] Update docs (#552) * Add missing controllers to doc index * Fix wrong forward_command controller doc --- doc/controllers_index.rst | 8 +++++--- forward_command_controller/doc/userdoc.rst | 8 ++++++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5eccbd9841..15d4bdd631 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -40,9 +40,11 @@ Available Controllers .. toctree:: :titlesonly: - Differential Drive <../diff_drive_controller/doc/userdoc.rst> - Forward Command <../forward_command_controller/doc/userdoc.rst> - Joint Trajectory <../joint_trajectory_controller/doc/userdoc.rst> + Admittance Controller <../admittance_controller/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index ce653fe714..8532a2dee9 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -3,9 +3,13 @@ forward_command_controller ========================== -This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. +This is a base class implementing a feedforward controller. Specific implementations can be found in: + +- `Position Controllers <../position_controllers/doc/userdoc.rst>`_ +- `Velocity Controllers <../velocity_controllers/doc/userdoc.rst>`_ +- `Effort Controllers <../effort_controllers/doc/userdoc.rst>`_ Hardware interface type ----------------------- -These controllers work with joints using the "effort" command interface. +This controller can be used for every type of command interface. From 03dfb84a258f63484b5cd986547982404317a7ea Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 28 Mar 2023 20:46:14 +0200 Subject: [PATCH 234/524] [AdmittanceController] Addintional argument in methods of ControllerInterface (#553) * [AdmittanceController] Addintional argument in methods of ControllerInterface * Update admittance_controller.cpp --- .../include/admittance_controller/admittance_controller.hpp | 3 ++- admittance_controller/src/admittance_controller.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 976c877b6f..e4aef2cf3d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -97,7 +97,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt protected: std::vector on_export_reference_interfaces() override; - controller_interface::return_type update_reference_from_subscribers() override; + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; size_t num_joints_ = 0; std::vector command_joint_names_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1af4e1317f..573d8ba06a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -370,7 +370,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +controller_interface::return_type AdmittanceController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // update input reference from ros subscriber message if (!admittance_) From b70a8d0f685574f7e9647f8f4b4518097d505214 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 30 Mar 2023 07:42:27 +0200 Subject: [PATCH 235/524] Fix broken links (#554) --- forward_command_controller/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 8532a2dee9..1f3a47997a 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,9 +5,9 @@ forward_command_controller This is a base class implementing a feedforward controller. Specific implementations can be found in: -- `Position Controllers <../position_controllers/doc/userdoc.rst>`_ -- `Velocity Controllers <../velocity_controllers/doc/userdoc.rst>`_ -- `Effort Controllers <../effort_controllers/doc/userdoc.rst>`_ +- :ref:`position_controllers_userdoc` +- :ref:`velocity_controllers_userdoc` +- :ref:`effort_controllers_userdoc` Hardware interface type ----------------------- From 05011b465f10e6f67363de0b6ab7bee510bd8552 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 2 Apr 2023 08:56:12 +0100 Subject: [PATCH 236/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 53 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index f0ffaa5f41..0ad86e0867 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [AdmittanceController] Addintional argument in methods of ControllerInterface (`#553 `_) +* Removed auto param decl (`#546 `_) +* Contributors: Dr. Denis, GuiHome + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1c20abd5d8..64b942787c 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.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index e38c07b589..1363bdb535 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a0b8cf5bd0..b5d9285840 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.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d87577328b..512f3027e9 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix broken links (`#554 `_) +* Update docs (`#552 `_) +* Contributors: Christoph Fröhlich + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 9291bb6098..b52afc1292 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a2bd1899f5..722dab7434 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.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8502816fd7..dd3aac0935 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.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5ab2f01fcd..31b6ff904e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update JTC documentation (`#541 `_) +* Contributors: Christoph Fröhlich + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ef05e136bd..ec15b1625d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8933ec9141..ce928074fb 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 157b9a853c..e1347215c6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 8c44f8ab1d..0aa39f89b8 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.3.0 (2023-03-07) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c86256d5e5..cda719df02 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Use std::clamp instead of rcppmath::clamp (`#540 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 11291b3b5d..3ba330db77 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ From ccc4ab333dd0e0ffcc05d51ea71b771b175f8162 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 2 Apr 2023 08:56:45 +0100 Subject: [PATCH 237/524] 3.4.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0ad86e0867..aaee61bb37 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ * [AdmittanceController] Addintional argument in methods of ControllerInterface (`#553 `_) * Removed auto param decl (`#546 `_) * Contributors: Dr. Denis, GuiHome diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 1b7a90e7e7..047e42b098 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.3.0 + 3.4.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 64b942787c..4265863ac9 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 16973cf821..db993d32cf 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.3.0 + 3.4.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 1363bdb535..8fcd65c3dc 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index c72b974644..9f988f052e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.3.0 + 3.4.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 b5d9285840..819d48b324 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index fd6a0de5bf..6db14c62e1 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.3.0 + 3.4.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 512f3027e9..0ea80f13fe 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.4.0 (2023-04-02) +------------------ * Fix broken links (`#554 `_) * Update docs (`#552 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 388b931a93..a490369e96 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b52afc1292..18dc5cf460 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index cc61c5d105..7cb3ef3a89 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.3.0 + 3.4.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 722dab7434..ed969086e6 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index aa5d4ffa6e..e28a48d3c2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.3.0 + 3.4.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 dd3aac0935..331862cb96 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e8af283875..19e545e44f 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.3.0 + 3.4.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 31b6ff904e..998463f27a 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.4.0 (2023-04-02) +------------------ * Update JTC documentation (`#541 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 7c1f9c74c1..59ba2a291d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.3.0 + 3.4.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 ec15b1625d..509f562201 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 180348f776..eb524c526a 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ce928074fb..5ede96124f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 429efd1aa0..973d2dc759 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.3.0 + 3.4.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 e1347215c6..409fb0ec91 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 01a961571a..c481adf3c7 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.3.0 + 3.4.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 a30e10b9df..031e08e22e 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.3.0", + version="3.4.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 0aa39f89b8..eab9296f8c 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.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 9ffac65a15..a2a54c29e5 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.3.0 + 3.4.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 62259dcf39..e3fc02453b 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.3.0", + version="3.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cda719df02..a5c51feece 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0c89f6dc05..c33f099d0c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.3.0 + 3.4.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3ba330db77..ac39176a5e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e197b29f72..1fe9aee0d5 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 8029a2f11aca838cabdd7a8bbe028dd75c334316 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 20:05:32 +0200 Subject: [PATCH 238/524] [JTC] Disable use of closed-loop PID adapter if controller is used in open-loop mode. (#551) --- joint_trajectory_controller/doc/userdoc.rst | 18 ++++++++++++++---- .../src/joint_trajectory_controller.cpp | 3 ++- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index e3d5c1976a..fc223d2eb0 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -142,11 +142,18 @@ interpolation_method (string) Default: splines open_loop_control (boolean) - Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + Use controller in open-loop control mode: + + The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. + + It deactivates the feedback control, see the ``gains`` structure. - Default: false + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + + .. Note:: + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. + Default: false constraints (structure) Default values for tolerances if no explicit values are states in JointTrajectory message. @@ -172,7 +179,10 @@ constraints..goal (double) Default: 0.0 (tolerance is not enforced) gains (structure) - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. This structure contains the controller gains for every joint with the control law + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law .. math:: diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bb26a9bb46..5c719f7471 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -599,7 +599,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // if there is only velocity or if there is effort command interface // then use also PID adapter use_closed_loop_pid_adapter_ = - (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + !params_.open_loop_control) || has_effort_command_interface_; if (use_closed_loop_pid_adapter_) From 1bad1e37637010774170fe9d965cbc0705422893 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 5 Apr 2023 15:31:43 +0200 Subject: [PATCH 239/524] [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (#561) --- .../src/admittance_controller_parameters.yaml | 2 +- .../src/gripper_action_controller_parameters.yaml | 6 +++--- .../src/joint_trajectory_controller_parameters.yaml | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5bdc40e398..ee1efa67ab 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -142,7 +142,7 @@ admittance_controller: description: "Specifies the joint damping applied used in the admittance calculation.", default_value: 5, validation: { - lower_bounds: [ 0.0 ] + gt_eq: [ 0.0 ] } } diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml index 9027e5b730..a847643b9f 100644 --- a/gripper_controllers/src/gripper_action_controller_parameters.yaml +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -4,7 +4,7 @@ gripper_action_controller: default_value: 20.0, description: "Hz", validation: { - lower_bounds: [0.1] + gt_eq: [0.1] }, } joint: { @@ -15,7 +15,7 @@ gripper_action_controller: type: double, default_value: 0.01, validation: { - lower_bounds: [0.0] + gt_eq: [0.0] }, } max_effort: { @@ -23,7 +23,7 @@ gripper_action_controller: default_value: 0.0, description: "Max allowable effort", validation: { - lower_bounds: [0.0] + gt_eq: [0.0] }, } allow_stalling: { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 2e0d2018f8..90ca12a268 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -55,7 +55,7 @@ joint_trajectory_controller: default_value: 20.0, description: "Rate status changes will be monitored", validation: { - lower_bounds: [0.1] + gt_eq: [0.1] } } interpolation_method: { @@ -109,7 +109,7 @@ joint_trajectory_controller: default_value: 0.0, description: "Time tolerance for achieving trajectory goal before or after commanded time.", validation: { - lower_bounds: [0.0], + gt_eq: [0.0], } } __map_joints: From 0ef3c1b2d548c7e83c3d617dd222cb3811c63c24 Mon Sep 17 00:00:00 2001 From: GuiHome Date: Wed, 5 Apr 2023 15:40:50 +0200 Subject: [PATCH 240/524] Misplaced param init in admittance_controller (#547) --- admittance_controller/src/admittance_controller.cpp | 12 ------------ .../test/test_admittance_controller.cpp | 8 +++----- .../test/test_admittance_controller.hpp | 2 ++ 3 files changed, 5 insertions(+), 17 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 573d8ba06a..c6a8168736 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -147,18 +147,6 @@ AdmittanceController::on_export_reference_interfaces() controller_interface::CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - try - { - parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - command_joint_names_ = admittance_->parameters_.command_joints; if (command_joint_names_.empty()) { diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..f1c3de8abd 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,16 +21,14 @@ #include #include -// Test on_configure returns ERROR when a required parameter is missing -TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +// Test on_init returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_init_parameter_is_missing) { ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedMissingParameters, + MissingMandatoryParameterDuringInit, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "chainable_command_interfaces", "command_interfaces", "control.frame.id", diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 499aec7de9..7dca2e2010 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -53,6 +53,8 @@ const double COMMON_THRESHOLD = 0.001; constexpr auto NODE_SUCCESS = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_FAILURE = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; From 1508d45753d9848e45ad414b19f9958a4cc9a0b1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 5 Apr 2023 15:53:16 +0200 Subject: [PATCH 241/524] [CI] Fixup the repos file for humble and rolling. (#562) --- ros2_controllers-not-released.humble.repos | 4 ---- ros2_controllers.humble.repos | 2 +- ros2_controllers.rolling.repos | 2 +- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index b9cdead153..1b3910e7e7 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,7 +4,3 @@ repositories: # type: git # url: git@github.com:/.git # version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: humble diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 8c0ba1ff8a..2a3b96551d 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -2,7 +2,7 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: humble realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index dfed17059b..42ec854e25 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -10,7 +10,7 @@ repositories: control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git - version: humble + version: master control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git From fa7499f0eb28ccca11408e703b8d5fce4f4deb30 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 7 Apr 2023 20:50:24 +0100 Subject: [PATCH 242/524] Remove RHEL from PRs (#564) --- .github/workflows/rolling-rhel-binary-build.yml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index c418319cd7..91e645d9e3 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,11 +1,6 @@ name: Rolling RHEL Binary Build on: workflow_dispatch: - branches: - - master - pull_request: - branches: - - master push: branches: - master From 9f013cc93f77cb5fee7e48a29f8be2f7bf898b57 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 14 Apr 2023 18:51:38 +0100 Subject: [PATCH 243/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 6 ++++++ 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 | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 53 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index aaee61bb37..ccd68d1b69 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Misplaced param init in admittance_controller (`#547 `_) +* [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) +* Contributors: Dr. Denis, GuiHome + 3.4.0 (2023-04-02) ------------------ * [AdmittanceController] Addintional argument in methods of ControllerInterface (`#553 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4265863ac9..8c79b5202d 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.4.0 (2023-04-02) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 8fcd65c3dc..965d261408 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 819d48b324..b2e216be9f 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.4.0 (2023-04-02) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 0ea80f13fe..a9506f9b0a 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.4.0 (2023-04-02) ------------------ * Fix broken links (`#554 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 18dc5cf460..07f5c34088 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) +* Contributors: Dr. Denis + 3.4.0 (2023-04-02) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ed969086e6..cf061dcce2 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.4.0 (2023-04-02) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 331862cb96..7cf6fe6248 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.4.0 (2023-04-02) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 998463f27a..3952c659ad 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) +* [JTC] Disable use of closed-loop PID adapter if controller is used in open-loop mode. (`#551 `_) +* Contributors: Dr. Denis + 3.4.0 (2023-04-02) ------------------ * Update JTC documentation (`#541 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 509f562201..67b7b63a76 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5ede96124f..c385511abe 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 409fb0ec91..0bef349cdd 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index eab9296f8c..7c11677cc8 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.4.0 (2023-04-02) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a5c51feece..0cf955a431 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ac39176a5e..c48f17db99 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.4.0 (2023-04-02) ------------------ From 48cc202108d4c91af2556176cae0c796f31d40fa Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 14 Apr 2023 18:52:07 +0100 Subject: [PATCH 244/524] 3.5.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ccd68d1b69..b7d6801e3c 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ * Misplaced param init in admittance_controller (`#547 `_) * [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) * Contributors: Dr. Denis, GuiHome diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 047e42b098..65f2b9c9b2 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.4.0 + 3.5.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8c79b5202d..1d2f0a3fbd 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index db993d32cf..96050a2f01 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.4.0 + 3.5.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 965d261408..3611be1381 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 9f988f052e..24228506a7 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.4.0 + 3.5.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 b2e216be9f..a1f9adbadd 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 6db14c62e1..df76d575c6 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.4.0 + 3.5.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 a9506f9b0a..fa71f9125a 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index a490369e96..f05ea322c5 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.4.0 + 3.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 07f5c34088..d390f60ed8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ * [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) * Contributors: Dr. Denis diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 7cb3ef3a89..770c4b257a 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.4.0 + 3.5.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index cf061dcce2..2de70959cf 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index e28a48d3c2..19ff483aba 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.4.0 + 3.5.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 7cf6fe6248..39d94ad9fd 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 19e545e44f..3f8b0ee517 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.4.0 + 3.5.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 3952c659ad..b153636ec0 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.5.0 (2023-04-14) +------------------ * [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) * [JTC] Disable use of closed-loop PID adapter if controller is used in open-loop mode. (`#551 `_) * Contributors: Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 59ba2a291d..f8e3908657 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.4.0 + 3.5.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 67b7b63a76..428b6b5f1d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index eb524c526a..c16c0895dc 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.4.0 + 3.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index c385511abe..5629c27780 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 973d2dc759..799b733945 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.4.0 + 3.5.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 0bef349cdd..638312ac61 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c481adf3c7..af7b40c799 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.4.0 + 3.5.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 031e08e22e..dc28906b5e 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.4.0", + version="3.5.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 7c11677cc8..5cac7c803a 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.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index a2a54c29e5..5d9bd8215e 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.4.0 + 3.5.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 e3fc02453b..81db8a055a 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.4.0", + version="3.5.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0cf955a431..d73a931b9b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index c33f099d0c..3bb3095ddd 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.4.0 + 3.5.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c48f17db99..538a095495 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.5.0 (2023-04-14) +------------------ 3.4.0 (2023-04-02) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 1fe9aee0d5..50c8dfa619 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.4.0 + 3.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 467bc4adffaaf10c07d87f517bd729a9d6e8f4bd Mon Sep 17 00:00:00 2001 From: muritane <31107191+muritane@users.noreply.github.com> Date: Wed, 19 Apr 2023 16:30:34 +0200 Subject: [PATCH 245/524] adjusted open_loop param description in diff_drive_controller_parameter.yaml (#570) --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index fb50f2fb50..f909b27e8b 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -62,7 +62,7 @@ diff_drive_controller: open_loop: { type: bool, default_value: false, - description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", } position_feedback: { type: bool, From 74c9e33b43570e6c87ba70e35a1e616301b7299d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 20 Apr 2023 17:30:55 +0200 Subject: [PATCH 246/524] Bump codecov/codecov-action from 3.1.1 to 3.1.2 (#575) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.1 to 3.1.2. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.1...v3.1.2) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index f7854aae5b..043f09dce3 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.1 + - uses: codecov/codecov-action@v3.1.2 with: file: ros_ws/lcov/total_coverage.info flags: unittests From b1dcab1469fa8bac46be942420e64a6effaa23f6 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 27 Apr 2023 17:17:21 +0100 Subject: [PATCH 247/524] Bump codecov/codecov-action from 3.1.2 to 3.1.3 (#581) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.2 to 3.1.3. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.2...v3.1.3) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 043f09dce3..6e6e96f032 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.2 + - uses: codecov/codecov-action@v3.1.3 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 5f076e2da11561f028b8d74900b21401cf001ed0 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 27 Apr 2023 17:17:38 +0100 Subject: [PATCH 248/524] Bump actions/setup-python from 4.5.0 to 4.6.0 (#582) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 9816e137c6..f0993bf717 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.5.0 + - uses: actions/setup-python@v4.6.0 with: python-version: '3.10' - name: Install system hooks From cf2723189f175a31201c251d1fa78277e105d52c Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 28 Apr 2023 12:27:06 +0200 Subject: [PATCH 249/524] Switch to recommended versions of ros-tooling (#577) --- .github/workflows/ci-ros-lint.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 5dd0b6f086..fd3e6b634b 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -41,8 +41,8 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 - - uses: ros-tooling/action-ros-lint@v0.1 + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master with: distribution: rolling linter: cpplint From edd494ca177b15838f3a5e1af5931d06b1129ec7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 28 Apr 2023 21:11:32 +0200 Subject: [PATCH 250/524] [JTC] Implement new ~/controller_state message (#557) --- joint_trajectory_controller/doc/userdoc.rst | 2 +- .../joint_trajectory_controller.hpp | 4 + .../src/joint_trajectory_controller.cpp | 146 ++++++++++++++-- .../test/test_trajectory_controller.cpp | 162 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 11 +- 5 files changed, 251 insertions(+), 74 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index fc223d2eb0..2370e9ca20 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -230,7 +230,7 @@ ROS2 interface of the controller ~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] Topic for commanding the controller. -~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] +~/controller_state (output topic) [control_msgs::msg::JointTrajectoryControllerState] Topic publishing internal states with the update-rate of the controller manager. ~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] 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 2a6454f454..dfbdf7436e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -127,6 +127,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_current_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -256,6 +257,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void read_state_from_hardware(JointTrajectoryPoint & state); bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); void query_state_service( const std::shared_ptr request, @@ -267,6 +269,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + void resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5c719f7471..b6a60526fa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -467,6 +467,81 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +bool JointTrajectoryController::read_commands_from_command_interfaces( + JointTrajectoryPoint & commands) +{ + bool has_values = true; + + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + }; + + // Assign values from the command interfaces as command. + if (has_position_command_interface_) + { + if (interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(commands.positions, joint_command_interface_[0]); + } + else + { + commands.positions.clear(); + has_values = false; + } + } + if (has_velocity_command_interface_) + { + if (interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(commands.velocities, joint_command_interface_[1]); + } + else + { + commands.velocities.clear(); + has_values = false; + } + } + if (has_acceleration_command_interface_) + { + if (interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(commands.accelerations, joint_command_interface_[2]); + } + else + { + commands.accelerations.clear(); + has_values = false; + } + } + if (has_effort_command_interface_) + { + if (interface_has_values(joint_command_interface_[3])) + { + assign_point_from_interface(commands.effort, joint_command_interface_[3]); + } + else + { + commands.effort.clear(); + has_values = false; + } + } + + return has_values; +} + void JointTrajectoryController::query_state_service( const std::shared_ptr request, std::shared_ptr response) @@ -706,27 +781,44 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); - publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); state_publisher_->msg_.joint_names = params_.joints; - state_publisher_->msg_.desired.positions.resize(dof_); - state_publisher_->msg_.desired.velocities.resize(dof_); - state_publisher_->msg_.desired.accelerations.resize(dof_); - state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.reference.positions.resize(dof_); + state_publisher_->msg_.reference.velocities.resize(dof_); + state_publisher_->msg_.reference.accelerations.resize(dof_); + state_publisher_->msg_.feedback.positions.resize(dof_); state_publisher_->msg_.error.positions.resize(dof_); if (has_velocity_state_interface_) { - state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.feedback.velocities.resize(dof_); state_publisher_->msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.feedback.accelerations.resize(dof_); state_publisher_->msg_.error.accelerations.resize(dof_); } + if (has_position_command_interface_) + { + state_publisher_->msg_.output.positions.resize(dof_); + } + if (has_velocity_command_interface_) + { + state_publisher_->msg_.output.velocities.resize(dof_); + } + if (has_acceleration_command_interface_) + { + state_publisher_->msg_.output.accelerations.resize(dof_); + } + if (has_effort_command_interface_) + { + state_publisher_->msg_.output.effort.resize(dof_); + } + state_publisher_->unlock(); // action server configuration @@ -749,6 +841,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point_command(command_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); @@ -929,21 +1022,25 @@ void JointTrajectoryController::publish_state( if (state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; + state_publisher_->msg_.reference.positions = desired_state.positions; + state_publisher_->msg_.reference.velocities = desired_state.velocities; + state_publisher_->msg_.reference.accelerations = desired_state.accelerations; + state_publisher_->msg_.feedback.positions = current_state.positions; state_publisher_->msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) { - state_publisher_->msg_.actual.velocities = current_state.velocities; + state_publisher_->msg_.feedback.velocities = current_state.velocities; state_publisher_->msg_.error.velocities = state_error.velocities; } if (has_acceleration_state_interface_) { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.feedback.accelerations = current_state.accelerations; state_publisher_->msg_.error.accelerations = state_error.accelerations; } + if (read_commands_from_command_interfaces(command_current_)) + { + state_publisher_->msg_.output = command_current_; + } state_publisher_->unlockAndPublish(); } @@ -1315,6 +1412,27 @@ void JointTrajectoryController::resize_joint_trajectory_point( } } +void JointTrajectoryController::resize_joint_trajectory_point_command( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + if (has_position_command_interface_) + { + point.positions.resize(size, 0.0); + } + if (has_velocity_command_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_command_interface_) + { + point.accelerations.resize(size, 0.0); + } + if (has_effort_command_interface_) + { + point.effort.resize(size, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 1c0dbb5482..e8073086dc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -29,7 +29,6 @@ #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" -#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -49,10 +48,11 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/state.hpp" #include "std_msgs/msg/header.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_trajectory_controller_utils.hpp" + using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -469,38 +469,83 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_EQ(joint_names_[i], state->joint_names[i]); } - // No trajectory by default, no desired state or error - EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS); - EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS); + // No trajectory by default, no reference state or error + EXPECT_TRUE( + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); EXPECT_TRUE( - state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); - EXPECT_EQ(n_joints, state->actual.positions.size()); + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_joints, state->feedback.positions.size()); if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.velocities.empty()); + EXPECT_TRUE(state->feedback.velocities.empty()); } else { - EXPECT_EQ(n_joints, state->actual.velocities.size()); + EXPECT_EQ(n_joints, state->feedback.velocities.size()); } if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { - EXPECT_TRUE(state->actual.accelerations.empty()); + EXPECT_TRUE(state->feedback.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); + EXPECT_EQ(n_joints, state->feedback.accelerations.size()); } - std::vector zeros(3, 0); - EXPECT_EQ(state->error.positions, zeros); - EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); - EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.positions.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.positions.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.velocities.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.accelerations.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.effort.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.effort.size()); + } } // Floating-point value comparison threshold @@ -540,25 +585,25 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->desired.positions.size()); - EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); EXPECT_EQ(n_joints, state_msg->error.positions.size()); - // are the correct desired positions used? - EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); // no normalization of position error EXPECT_NEAR( - state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( - state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { @@ -566,28 +611,34 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); } if (traj_controller_->has_velocity_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_LT(0.0, state_msg->output.velocities[2]); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { // we expect u = k_p * (s_d-s) EXPECT_NEAR( - k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * allowed_delta); EXPECT_NEAR( - k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // no normalization of position error EXPECT_NEAR( - k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], k_p * allowed_delta); } } @@ -595,9 +646,12 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) if (traj_controller_->has_effort_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_LT(0.0, state_msg->output.effort[2]); } executor.cancel(); @@ -638,26 +692,26 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->desired.positions.size()); - EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); EXPECT_EQ(n_joints, state_msg->error.positions.size()); - // are the correct desired positions used? - EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); // is error.positions[2] normalized? EXPECT_NEAR( - state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( - state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( state_msg->error.positions[2], - state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); if (traj_controller_->has_position_command_interface()) { @@ -670,38 +724,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) if (traj_controller_->has_velocity_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - EXPECT_GE(0.0, joint_vel_[2]); + EXPECT_GT(0.0, joint_vel_[2]); // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * allowed_delta); EXPECT_NEAR( - k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // is error of positions[2] normalized? EXPECT_NEAR( - k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta); } else { // interpolated points_velocities only - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { // check command interface - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); } executor.cancel(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 25612f56e9..c85eb59f58 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -281,7 +281,7 @@ class TrajectoryControllerTest : public ::testing::Test // I do not understand why spin_some provides only one message qos.keep_last(1); state_subscriber_ = traj_lifecycle_node->create_subscription( - controller_name_ + "/state", qos, + controller_name_ + "/controller_state", qos, [&](std::shared_ptr msg) { std::lock_guard guard(state_mutex_); @@ -385,20 +385,21 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); - // TODO(anyone): add checking for velocties and accelerations + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); } } for (size_t i = 0; i < expected_desired.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); - // TODO(anyone): add checking for velocties and accelerations + // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + EXPECT_NEAR( + expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); } } } From 8da513533292b6a7add0d2026b6d718797df8010 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 29 Apr 2023 11:20:31 +0200 Subject: [PATCH 251/524] Fix docs format (#589) * Fix docs format * Fix trailing whitespace --- admittance_controller/doc/userdoc.rst | 5 +- doc/controllers_index.rst | 4 +- forward_command_controller/doc/userdoc.rst | 6 +-- joint_state_broadcaster/doc/userdoc.rst | 10 ++-- joint_trajectory_controller/doc/userdoc.rst | 57 +++++++++++---------- 5 files changed, 42 insertions(+), 40 deletions(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 60837457f9..ed2da84ad5 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -.. _joint_trajectory_controller_userdoc: +.. _addmittance_controller_userdoc: Admittance Controller ====================== @@ -8,9 +8,6 @@ The controller implements ``ChainedControllerInterface``, so it is possible to a The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. -Parameters: - - ROS2 interface of the controller --------------------------------- diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 15d4bdd631..1c6b1dbd9e 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -41,13 +41,13 @@ Available Controllers :titlesonly: Admittance Controller <../admittance_controller/doc/userdoc.rst> - Tricycle Controller <../tricycle_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> Available Broadcasters diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 1f3a47997a..8292bdc1b3 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,9 +5,9 @@ forward_command_controller This is a base class implementing a feedforward controller. Specific implementations can be found in: -- :ref:`position_controllers_userdoc` -- :ref:`velocity_controllers_userdoc` -- :ref:`effort_controllers_userdoc` +* :ref:`position_controllers_userdoc` +* :ref:`velocity_controllers_userdoc` +* :ref:`effort_controllers_userdoc` Hardware interface type ----------------------- diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 8a030d9356..fbdb439992 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -21,26 +21,26 @@ If none of the requested interface are not defined, the controller returns error Parameters ---------- -``use_local_topics`` +use_local_topics Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. -``joints`` +joints Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``interfaces`` parameter. Joint state broadcaster asks for access to all defined interfaces on all defined joints. -``interfaces`` +interfaces Optional parameter (string array) to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``joints`` parameter. -``extra_joints`` +extra_joints Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. -``map_interface_to_joint_state`` +map_interface_to_joint_state Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. Usecases: diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 2370e9ca20..1ba6257a5a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -10,11 +10,11 @@ Trajectory representation The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: - Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. +* Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. - Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. +* Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. - Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. +* Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. Hardware interface type ----------------------- @@ -26,11 +26,11 @@ Similarly to the trajectory representation case above, it's possible to support Other features -------------- - Realtime-safe implementation. +* Realtime-safe implementation. - Proper handling of wrapping (continuous) joints. +* Proper handling of wrapping (continuous) joints. - Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. +* Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. ros2_control interfaces ------------------------ @@ -45,10 +45,10 @@ The state interfaces are defined with ``joints`` and ``state_interfaces`` parame Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. Legal combinations of state interfaces are: -- ``position`` -- ``position`` and ``velocity`` -- ``position``, ``velocity`` and ``acceleration`` -- ``effort`` +* ``position`` +* ``position`` and ``velocity`` +* ``position``, ``velocity`` and ``acceleration`` +* ``effort`` Commands ^^^^^^^^^ @@ -244,23 +244,28 @@ The controller types are placed into namespaces according to their command types The following version of the Joint Trajectory Controller are available mapping the following interfaces: - - position_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position - - position_velocity_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position and velocity - - position_velocity_acceleration_controllers::JointTrajectoryController - - Input: position, [velocity, [acceleration]] - - Output: position, velocity and acceleration - -.. - velocity_controllers::JointTrajectoryController -.. - Input: position, [velocity, [acceleration]] -.. - Output: velocity +* position_controllers::JointTrajectoryController + + * Input: position, [velocity, [acceleration]] + * Output: position + +* position_velocity_controllers::JointTrajectoryController + + * Input: position, [velocity, [acceleration]] + * Output: position and velocity + +* position_velocity_acceleration_controllers::JointTrajectoryController + + * Input: position, [velocity, [acceleration]] + * Output: position, velocity and acceleration + +.. * velocity_controllers::JointTrajectoryController +.. * Input: position, [velocity, [acceleration]] +.. * Output: velocity .. TODO(anyone): would it be possible to output velocty and acceleration? .. (to have an vel_acc_controllers) -.. - effort_controllers::JointTrajectoryController -.. - Input: position, [velocity, [acceleration]] -.. - Output: effort +.. * effort_controllers::JointTrajectoryController +.. * Input: position, [velocity, [acceleration]] +.. * Output: effort (*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) From 1db184b012217158335f91cdf7521097c36f0777 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 21:23:10 +0100 Subject: [PATCH 252/524] Update reviewer-lottery.yml (#588) --- .github/reviewer-lottery.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 50c707e12b..84b156f5a1 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -16,13 +16,16 @@ groups: - rosterloh - progtologist - arne48 + - christophfroehlich - DasRoteSkelett - - Serafadam + - sgmurray - harderthan - jaron-l - malapatiravi - - homalozoa - erickisos + - sachinkum0009 + - qiayuanliao + - homalozoa - anfemosa - jackcenter - VX792 @@ -31,6 +34,7 @@ groups: - aprotyas - peterdavidfagan - duringhof + - VanshGehlot - bijoua29 - - lm2292 + - LukasMacha97 - mcbed From 1e3129d44c7781b5b9f36f48238477060cc92fba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 29 Apr 2023 22:23:59 +0200 Subject: [PATCH 253/524] Fix PR template (#587) --- .../pull_request_template.md | 10 ---------- 1 file changed, 10 deletions(-) rename .github/{PULL_REQUEST_TEMPLATE => }/pull_request_template.md (92%) diff --git a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md b/.github/pull_request_template.md similarity index 92% rename from .github/PULL_REQUEST_TEMPLATE/pull_request_template.md rename to .github/pull_request_template.md index 87f389a124..8e9178c167 100644 --- a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,13 +1,3 @@ - ---- -name: Pull request -about: Create a pull request -title: '' -labels: '' -assignees: '' - ---- - Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: 1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs. From 4037719b3342595ae2ff31d99dde44d55b33c788 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 22:22:19 +0100 Subject: [PATCH 254/524] Renovate load controller tests (#569) --- .../test/test_load_admittance_controller.cpp | 5 +++-- .../test/test_load_diff_drive_controller.cpp | 5 +++-- .../test/test_load_joint_group_effort_controller.cpp | 6 ++++-- .../test_load_force_torque_sensor_broadcaster.cpp | 8 +++++--- .../test/test_load_forward_command_controller.cpp | 6 ++++-- ...ad_multi_interface_forward_command_controller.cpp | 8 +++++--- .../test/test_load_gripper_action_controllers.cpp | 12 ++++++++---- .../test/test_load_imu_sensor_broadcaster.cpp | 6 ++++-- .../test/test_load_joint_state_broadcaster.cpp | 6 ++++-- .../test/test_load_joint_trajectory_controller.cpp | 6 ++++-- .../test_load_joint_group_position_controller.cpp | 6 ++++-- .../test/test_load_tricycle_controller.cpp | 7 ++++--- .../test_load_joint_group_velocity_controller.cpp | 6 ++++-- 13 files changed, 56 insertions(+), 31 deletions(-) diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 1f290aeff6..23be1f23f5 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -33,8 +33,9 @@ TEST(TestLoadAdmittanceController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); + ASSERT_EQ( + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), + nullptr); } int main(int argc, char ** argv) diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 20caf46b6f..1eb8939031 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -30,8 +30,9 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_unique(ros2_control_test_assets::diffbot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController")); + ASSERT_NE( + cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), + nullptr); rclcpp::shutdown(); } diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index a216d46db3..61bb1ddf9a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -32,8 +32,10 @@ TEST(TestLoadJointGroupVelocityController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController"), + nullptr); rclcpp::shutdown(); } diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 6e9b6b1167..a9ca5e88fc 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -38,9 +38,11 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_force_torque_sensor_broadcaster", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), + nullptr); rclcpp::shutdown(); } diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index 464b57b69d..b493e52b2a 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -34,8 +34,10 @@ TEST(TestLoadForwardCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); + ASSERT_NE( + cm.load_controller( + "test_forward_command_controller", "forward_command_controller/ForwardCommandController"), + nullptr); rclcpp::shutdown(); } diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 2f540bd0e5..41a9b74698 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -35,7 +35,9 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", - "forward_command_controller/MultiInterfaceForwardCommandController")); + ASSERT_NE( + cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/MultiInterfaceForwardCommandController"), + nullptr); } diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index b355cdf34a..130b12e0bb 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -32,10 +32,14 @@ TEST(TestLoadGripperActionControllers, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController")); - ASSERT_NO_THROW(cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController")); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); rclcpp::shutdown(); } diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index e90c9915ed..e7cfd1bcf7 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -38,8 +38,10 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), + nullptr); rclcpp::shutdown(); } diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index d7a2934f57..5efb587805 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -34,8 +34,10 @@ TEST(TestLoadJointStateBroadcaster, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster")); + ASSERT_NE( + cm.load_controller( + "test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster"), + nullptr); rclcpp::shutdown(); } diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 0f94d0b16c..eb1a3691e6 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -35,8 +35,10 @@ TEST(TestLoadJointStateController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController")); + ASSERT_NE( + cm.load_controller( + "test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController"), + nullptr); rclcpp::shutdown(); } diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index cc376bd2d4..fe61039fdb 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -32,8 +32,10 @@ TEST(TestLoadJointGroupPositionController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_position_controller", "position_controllers/JointGroupPositionController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_position_controller", "position_controllers/JointGroupPositionController"), + nullptr); rclcpp::shutdown(); } diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index d04b83ae27..9298fae574 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -16,7 +16,7 @@ * Author: Tony Najjar */ -#include +#include #include #include "controller_manager/controller_manager.hpp" @@ -36,8 +36,9 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NE( + cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), + nullptr); rclcpp::shutdown(); } diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 36d5a8368d..1872b5f746 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -32,8 +32,10 @@ TEST(TestLoadJointGroupVelocityController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController")); + ASSERT_NE( + cm.load_controller( + "test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController"), + nullptr); rclcpp::shutdown(); } From 3bb74e7fff99eda85eb5583b9ad7f733e62bcdeb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 22:37:12 +0100 Subject: [PATCH 255/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 75 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b7d6801e3c..a5621304b8 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Fix docs format (`#589 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 3.5.0 (2023-04-14) ------------------ * Misplaced param init in admittance_controller (`#547 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1d2f0a3fbd..a28cd0baea 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* adjusted open_loop param description in diff_drive_controller_parameter.yaml (`#570 `_) +* Contributors: Bence Magyar, muritane + 3.5.0 (2023-04-14) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3611be1381..355f6a0576 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a1f9adbadd..34450fa1cb 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index fa71f9125a..786962aa42 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Fix docs format (`#589 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 3.5.0 (2023-04-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d390f60ed8..c46a42bb3e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ * [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2de70959cf..e333525386 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 39d94ad9fd..084358ac4a 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Fix docs format (`#589 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 3.5.0 (2023-04-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index b153636ec0..b5797f1a3f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Fix docs format (`#589 `_) +* [JTC] Implement new ~/controller_state message (`#557 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 3.5.0 (2023-04-14) ------------------ * [Parameters] Use `gt_eq` instead of deprecated `lower_bounds` in validators (`#561 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 428b6b5f1d..3a3936bc61 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5629c27780..348f0df662 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.5.0 (2023-04-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 638312ac61..b1bb0e217c 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.5.0 (2023-04-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5cac7c803a..ce7da67d6d 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.5.0 (2023-04-14) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index d73a931b9b..f1dacc5a6f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 538a095495..794d2d32db 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Renovate load controller tests (`#569 `_) +* Contributors: Bence Magyar + 3.5.0 (2023-04-14) ------------------ From 00a8c2e3a2152fc7d627187d0e7434765f1777b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 29 Apr 2023 22:39:22 +0100 Subject: [PATCH 256/524] 3.6.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a5621304b8..775c6cb1f0 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Fix docs format (`#589 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 65f2b9c9b2..253848eeba 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.5.0 + 3.6.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a28cd0baea..2cb66cfd13 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * adjusted open_loop param description in diff_drive_controller_parameter.yaml (`#570 `_) * Contributors: Bence Magyar, muritane diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 96050a2f01..259e3a4835 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.5.0 + 3.6.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 355f6a0576..5d6b5ad33f 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 24228506a7..4b810f018f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.5.0 + 3.6.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 34450fa1cb..7aaedabd91 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index df76d575c6..884910a239 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.5.0 + 3.6.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 786962aa42..c6a70728c8 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Fix docs format (`#589 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f05ea322c5..f300283bea 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.5.0 + 3.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c46a42bb3e..ffbd89cdc8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 770c4b257a..1741f468bd 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.5.0 + 3.6.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e333525386..976b41ec54 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 19ff483aba..16a276b218 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.5.0 + 3.6.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 084358ac4a..14b1b2a517 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Fix docs format (`#589 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 3f8b0ee517..5a2c3665a3 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.5.0 + 3.6.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 b5797f1a3f..26e1447b0e 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.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Fix docs format (`#589 `_) * [JTC] Implement new ~/controller_state message (`#557 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f8e3908657..64a56b0375 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.5.0 + 3.6.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 3a3936bc61..e056c8544d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c16c0895dc..b1e0609a43 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.5.0 + 3.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 348f0df662..6cb91dc982 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ 3.5.0 (2023-04-14) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 799b733945..ad3ed5493d 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.5.0 + 3.6.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 b1bb0e217c..5e7a1de558 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.6.0 (2023-04-29) +------------------ 3.5.0 (2023-04-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index af7b40c799..44207cf598 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.5.0 + 3.6.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 dc28906b5e..b0050c83e5 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.5.0", + version="3.6.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 ce7da67d6d..7068d774ac 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.6.0 (2023-04-29) +------------------ 3.5.0 (2023-04-14) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 5d9bd8215e..f5d567edb5 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.5.0 + 3.6.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 81db8a055a..8e57dd1123 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.5.0", + version="3.6.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f1dacc5a6f..d84e66bb4e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3bb3095ddd..2072b334a7 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.5.0 + 3.6.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 794d2d32db..dca69ca6d1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.6.0 (2023-04-29) +------------------ * Renovate load controller tests (`#569 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 50c8dfa619..f3205e00cc 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.5.0 + 3.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 634e6fe03497bedd4ffa805c327a53f2fe297e5e Mon Sep 17 00:00:00 2001 From: Marq Rasmussen Date: Tue, 2 May 2023 11:40:25 -0600 Subject: [PATCH 257/524] Fix JTC from immediately returning success (#565) Co-authored-by: Bence Magyar --- .../src/joint_trajectory_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b6a60526fa..07080bb1fc 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -152,6 +152,8 @@ controller_interface::return_type JointTrajectoryController::update( sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity traj_external_point_ptr_->update(*new_external_msg); + // set the active trajectory pointer to the new goal + traj_point_active_ptr_ = &traj_external_point_ptr_; } // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not @@ -315,6 +317,8 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; // check goal tolerance } @@ -328,6 +332,8 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + // remove the active trajectory pointer so that we stop commanding the hardware + traj_point_active_ptr_ = nullptr; RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); } From 7cb412887c79ff405103c57fd8fd08aaedf33365 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= <48183611+Noel215@users.noreply.github.com> Date: Tue, 2 May 2023 23:14:53 +0200 Subject: [PATCH 258/524] Fix wrong publish timestamp initialization (#585) * Fix publish timestamp Signed-off-by: Noel Jimenez * Document catch purpose Signed-off-by: Noel Jimenez --------- Signed-off-by: Noel Jimenez --- .../diff_drive_controller.hpp | 2 +- .../src/diff_drive_controller.cpp | 19 +++++++++++++++++-- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 277c7bd99b..5923a27d4d 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 @@ -148,7 +148,7 @@ class DiffDriveController : public controller_interface::ControllerInterface // publish rate limiter double publish_rate_ = 50.0; rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; bool is_halted = false; bool use_stamped_vel_ = true; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index b6d9b07a49..22b9acd829 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -185,7 +185,23 @@ controller_interface::return_type DiffDriveController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < time) + bool should_publish = false; + try + { + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + should_publish = true; + } + } + catch (const std::runtime_error) + { + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; + should_publish = true; + } + + if (should_publish) { previous_publish_timestamp_ += publish_period_; @@ -400,7 +416,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // limit the publication on the topics /odom and /tf publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - previous_publish_timestamp_ = get_node()->get_clock()->now(); // initialize odom values zeros odometry_message.twist = From 34008468dbb702278e26b6d65cb8bf1c411b8ee5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 2 May 2023 22:22:32 +0100 Subject: [PATCH 259/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 49 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 775c6cb1f0..65ab9c3fab 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 2cb66cfd13..ead77a8092 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix wrong publish timestamp initialization (`#585 `_) +* Contributors: Noel Jiménez García + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 5d6b5ad33f..dc27b09976 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7aaedabd91..faf8141487 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.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index c6a70728c8..025e0e8b3c 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.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ffbd89cdc8..8d91e270d3 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 976b41ec54..e39f245d37 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.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 14b1b2a517..73a8fc2d0b 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.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 26e1447b0e..987beab601 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix JTC from immediately returning success (`#565 `_) +* Contributors: Marq Rasmussen + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e056c8544d..0ff0f42f79 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6cb91dc982..46094516e2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 5e7a1de558..8cbec95721 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7068d774ac..6d488c190c 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.6.0 (2023-04-29) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index d84e66bb4e..c6e39022fa 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index dca69ca6d1..745b6cb87f 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.6.0 (2023-04-29) ------------------ * Renovate load controller tests (`#569 `_) From 6793e0278f35b34800c14a085188f3d7a3b4f6ad Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 2 May 2023 22:23:06 +0100 Subject: [PATCH 260/524] 3.7.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 65ab9c3fab..63ca4f4c0c 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 253848eeba..022e8e1353 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.6.0 + 3.7.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index ead77a8092..272656ce20 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.7.0 (2023-05-02) +------------------ * Fix wrong publish timestamp initialization (`#585 `_) * Contributors: Noel Jiménez García diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 259e3a4835..0fdbb3a8a5 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.6.0 + 3.7.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 dc27b09976..767184b5d3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 4b810f018f..02fc8594c0 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.6.0 + 3.7.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 faf8141487..672d2b53ca 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 884910a239..f7ab4d61ab 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.6.0 + 3.7.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 025e0e8b3c..bf3b24ab82 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f300283bea..4441a019ab 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.6.0 + 3.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8d91e270d3..b7378fa760 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 1741f468bd..4d53c64d7f 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.6.0 + 3.7.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e39f245d37..9049c11f6a 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 16a276b218..b5e9e6c9f8 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.6.0 + 3.7.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 73a8fc2d0b..3812d61ac3 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 5a2c3665a3..5ad73b4ea0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.6.0 + 3.7.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 987beab601..fbb24978b7 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.7.0 (2023-05-02) +------------------ * Fix JTC from immediately returning success (`#565 `_) * Contributors: Marq Rasmussen diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 64a56b0375..00cad650c4 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.6.0 + 3.7.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 0ff0f42f79..3b04f3b06f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b1e0609a43..b1ca3b38b7 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.6.0 + 3.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 46094516e2..3397d446e8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ad3ed5493d..b2b7fe286c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.6.0 + 3.7.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 8cbec95721..69e4765398 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 44207cf598..3fee344465 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.6.0 + 3.7.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 b0050c83e5..3cdf9a76ef 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.6.0", + version="3.7.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 6d488c190c..b9c0b090d0 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.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index f5d567edb5..86196cac41 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.6.0 + 3.7.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 8e57dd1123..f04123ec65 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.6.0", + version="3.7.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c6e39022fa..bf0c5a0102 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 2072b334a7..83cb39cc7f 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.6.0 + 3.7.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 745b6cb87f..4d584c209f 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.7.0 (2023-05-02) +------------------ 3.6.0 (2023-04-29) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f3205e00cc..4bbf7a2723 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.6.0 + 3.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 0ffadb8840bdca0c4658cbf3539f6ee4e1057131 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Sun, 7 May 2023 17:46:00 -0300 Subject: [PATCH 261/524] [EOL] Drop EOL Galactic from github actions (#598) Galactic EOL Date: December 9th, 2022 https://docs.ros.org/en/rolling/Releases.html - remove all galactic repo files - remove all galactic workflows - remove all galactic status badges Signed-off-by: Alex Moriarty --- .github/mergify.yml | 9 ----- .github/workflows/README.md | 1 - .../workflows/galactic-abi-compatibility.yml | 20 ----------- .../workflows/galactic-binary-build-main.yml | 26 --------------- .../galactic-binary-build-testing.yml | 26 --------------- .../workflows/galactic-rhel-binary-build.yml | 33 ------------------- .../galactic-semi-binary-build-main.yml | 25 -------------- .../galactic-semi-binary-build-testing.yml | 25 -------------- .github/workflows/prerelease-check.yml | 2 -- .../reusable-ros-tooling-source-build.yml | 2 +- README.md | 1 - ros2_controllers-not-released.galactic.repos | 6 ---- ros2_controllers.galactic.repos | 13 -------- 13 files changed, 1 insertion(+), 188 deletions(-) delete mode 100644 .github/workflows/galactic-abi-compatibility.yml delete mode 100644 .github/workflows/galactic-binary-build-main.yml delete mode 100644 .github/workflows/galactic-binary-build-testing.yml delete mode 100644 .github/workflows/galactic-rhel-binary-build.yml delete mode 100644 .github/workflows/galactic-semi-binary-build-main.yml delete mode 100644 .github/workflows/galactic-semi-binary-build-testing.yml delete mode 100644 ros2_controllers-not-released.galactic.repos delete mode 100644 ros2_controllers.galactic.repos diff --git a/.github/mergify.yml b/.github/mergify.yml index 6bfd4470ac..87490702f6 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,13 +1,4 @@ pull_request_rules: - - name: Backport to galactic at reviewers discretion - conditions: - - base=master - - "label=backport-galactic" - actions: - backport: - branches: - - galactic - - name: Backport to foxy at reviewers discretion conditions: - base=master diff --git a/.github/workflows/README.md b/.github/workflows/README.md index 34bfb4cc03..5789859350 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -4,7 +4,6 @@ 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) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml deleted file mode 100644 index 06a48ef9c7..0000000000 --- a/.github/workflows/galactic-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Galactic - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: galactic - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build-main.yml b/.github/workflows/galactic-binary-build-main.yml deleted file mode 100644 index 14fe56407c..0000000000 --- a/.github/workflows/galactic-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Galactic Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - 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: galactic - ros_repo: main - upstream_workspace: ros2_controllers-not-released.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-binary-build-testing.yml b/.github/workflows/galactic-binary-build-testing.yml deleted file mode 100644 index c4b22a3a7a..0000000000 --- a/.github/workflows/galactic-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Galactic Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - 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: galactic - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml deleted file mode 100644 index 518fcc186a..0000000000 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Galactic RHEL Binary Build -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - galactic_rhel_binary: - name: Galactic RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: galactic - container: ghcr.io/ros-controls/ros:galactic-rhel - steps: - - uses: actions/checkout@v3 - 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/galactic-semi-binary-build-main.yml b/.github/workflows/galactic-semi-binary-build-main.yml deleted file mode 100644 index 7e498679cb..0000000000 --- a/.github/workflows/galactic-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Galactic Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - 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: galactic - ros_repo: main - upstream_workspace: ros2_controllers.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-semi-binary-build-testing.yml b/.github/workflows/galactic-semi-binary-build-testing.yml deleted file mode 100644 index 82a74a358b..0000000000 --- a/.github/workflows/galactic-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Galactic Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - galactic - pull_request: - branches: - - galactic - push: - branches: - - galactic - 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: galactic - ros_repo: testing - upstream_workspace: ros2_controllers.galactic.repos - ref_for_scheduled_build: galactic diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index b9460bda47..acbb42e16b 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -10,7 +10,6 @@ on: type: choice options: - foxy - - galactic - humble - rolling branch: @@ -20,7 +19,6 @@ on: type: choice options: - foxy - - galactic - humble - master diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 51519dfb2c..31bc0d4475 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -14,7 +14,7 @@ on: required: true type: string ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, foxy.' default: 'master' required: false type: string diff --git a/README.md b/README.md index 6a59d0054e..d7986d34c6 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,6 @@ 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) **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) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.galactic.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos deleted file mode 100644 index b9b8674fc9..0000000000 --- a/ros2_controllers.galactic.repos +++ /dev/null @@ -1,13 +0,0 @@ -repositories: - ros-controls/ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: galactic - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel From 546422d8639082fea5d224511505e281e8566ecf Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Sun, 7 May 2023 17:46:38 -0300 Subject: [PATCH 262/524] switch from dash to underscore in setup.cfg (#595) This fixes the warning which was introduced here: https://github.com/pypa/setuptools/commit/a2e9ae4cb Signed-off-by: Alex Moriarty --- rqt_joint_trajectory_controller/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rqt_joint_trajectory_controller/setup.cfg b/rqt_joint_trajectory_controller/setup.cfg index 4c0b982e25..59deadec31 100644 --- a/rqt_joint_trajectory_controller/setup.cfg +++ b/rqt_joint_trajectory_controller/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/rqt_joint_trajectory_controller +script_dir=$base/lib/rqt_joint_trajectory_controller [install] -install-scripts=$base/lib/rqt_joint_trajectory_controller +install_scripts=$base/lib/rqt_joint_trajectory_controller From 6851df137cf2118d1a7166921d34eb759c225526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 9 May 2023 19:02:17 +0200 Subject: [PATCH 263/524] [JTC] Import docs from wiki.ros.org (#566) * Remove trajectory replacement doc from this PR Co-authored-by: Dr. Denis --- joint_trajectory_controller/doc/userdoc.rst | 180 +++++++++++++------- 1 file changed, 121 insertions(+), 59 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 1ba6257a5a..6494d7116a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -3,12 +3,22 @@ joint_trajectory_controller =========================== -Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. +Controller for executing joint-space trajectories on a group of joints. +The controller interpolates in time between the points so that their distance can be arbitrary. +Even trajectories with only one point are accepted. +Trajectories are specified as a set of waypoints to be reached at specific time instants, +which the controller attempts to execute as well as the mechanism allows. +Waypoints consist of positions, and optionally velocities and accelerations. -Trajectory representation -------------------------- -The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ [#f2]_ + +Trajectory representation [#f1]_ +--------------------------------- + +Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. +By default, a spline interpolator is provided, but it's possible to support other representations. +The spline interpolator uses the following interpolation strategies depending on the waypoint specification: * Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. @@ -16,10 +26,27 @@ The controller is templated to work with multiple trajectory representations. By * Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. -Hardware interface type ------------------------ +Hardware interface type [#f1]_ +------------------------------- + +Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time: + +* For command interfaces ``position``, the desired positions are simply forwarded to the joints, +* For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`). + +This leads to the the following allowed combinations of command and state interfaces: -The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here. +* With command interface ``position``, there are no restrictions for 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``, there are no restrictions for state interfaces. + +Example controller configurations can be found :ref:`below `. Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). @@ -32,27 +59,6 @@ Other features * Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. -ros2_control interfaces ------------------------- - -References -^^^^^^^^^^^ -(the controller is not yet implemented as chainable controller) - -States -^^^^^^^ -The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. -Legal combinations of state interfaces are: - -* ``position`` -* ``position`` and ``velocity`` -* ``position``, ``velocity`` and ``acceleration`` -* ``effort`` - -Commands -^^^^^^^^^ - Using Joint Trajectory Controller(s) ------------------------------------ @@ -101,6 +107,17 @@ A yaml file for using it could be: goal: 0.03 +Preemption policy [#f1]_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Only one action goal can be active at any moment, or none if the topic interface is used. Path and goal tolerances are checked only for the trajectory segments of the active goal. + +When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. + +Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. + +.. _parameters: + Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ @@ -143,8 +160,9 @@ interpolation_method (string) open_loop_control (boolean) Use controller in open-loop control mode: - + The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - + It deactivates the feedback control, see the ``gains`` structure. + + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. + * It deactivates the feedback control, see the ``gains`` structure. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). @@ -224,17 +242,79 @@ gains..normalize_error (bool) Default: false -ROS2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] - Topic for commanding the controller. +.. _ROS 2 interface: -~/controller_state (output topic) [control_msgs::msg::JointTrajectoryControllerState] - Topic publishing internal states with the update-rate of the controller manager. +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] - Action server for commanding the controller. +References +,,,,,,,,,,,,,,,,,, + +(the controller is not yet implemented as chainable controller) + +States +,,,,,,,,,,,,,,,,,, + +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. + +Legal combinations of state interfaces are: + +* ``position`` +* ``position`` and ``velocity`` +* ``position``, ``velocity`` and ``acceleration`` +* ``effort`` + +Commands +,,,,,,,,, + +There are two mechanisms for sending trajectories to the controller: + +* via action, see :ref:`actions ` +* via topic, see :ref:`subscriber ` + +Both use the ``trajectory_msgs/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. + +.. _Actions: + +Actions [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +/follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] + Action server for commanding the controller + + +The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). +If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. + +.. _Subscriber: + +Subscriber [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +/joint_trajectory [trajectory_msgs::msg::JointTrajectory] + Topic for commanding the controller + +The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. +The controller's path and goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. +Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. + + +Publishers +,,,,,,,,,,, + +/controller_state [control_msgs::msg::JointTrajectoryControllerState] + Topic publishing internal states with the update-rate of the controller manager + + +Services +,,,,,,,,,,, + +/query_state [control_msgs::srv::QueryTrajectoryState] + Query controller state at any future time Specialized versions of JointTrajectoryController (TBD in ...) @@ -246,26 +326,8 @@ The following version of the Joint Trajectory Controller are available mapping t * position_controllers::JointTrajectoryController - * Input: position, [velocity, [acceleration]] - * Output: position - -* position_velocity_controllers::JointTrajectoryController - - * Input: position, [velocity, [acceleration]] - * Output: position and velocity - -* position_velocity_acceleration_controllers::JointTrajectoryController - - * Input: position, [velocity, [acceleration]] - * Output: position, velocity and acceleration -.. * velocity_controllers::JointTrajectoryController -.. * Input: position, [velocity, [acceleration]] -.. * Output: velocity -.. TODO(anyone): would it be possible to output velocty and acceleration? -.. (to have an vel_acc_controllers) -.. * effort_controllers::JointTrajectoryController -.. * Input: position, [velocity, [acceleration]] -.. * Output: effort +.. rubric:: Footnote -(*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) +.. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ +.. [#f2] Adolfo Rodriguez: `Understanding trajectory replacement `_ From 83042fbb9e4cfed9c0a06d3dc6bda5d070c8a138 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 11 May 2023 15:37:40 +0200 Subject: [PATCH 264/524] Clear registered handles of DiffDriveController on deactivate (#596) --- diff_drive_controller/src/diff_drive_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 22b9acd829..fa12bd8105 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -481,6 +481,13 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; + if (!is_halted) + { + halt(); + is_halted = true; + } + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; } From b94cb58ea94a5616102ee78da07d070af9643f09 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 14 May 2023 19:49:13 +0100 Subject: [PATCH 265/524] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 51 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 63ca4f4c0c..dd22bd8892 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 272656ce20..bbab2ab10d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Clear registered handles of DiffDriveController on deactivate (`#596 `_) +* Contributors: Noel Jiménez García + 3.7.0 (2023-05-02) ------------------ * Fix wrong publish timestamp initialization (`#585 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 767184b5d3..63ec09e4c4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 672d2b53ca..a17b54b2c0 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.7.0 (2023-05-02) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index bf3b24ab82..b714c9884c 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.7.0 (2023-05-02) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b7378fa760..40aa2169ff 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9049c11f6a..445341daab 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.7.0 (2023-05-02) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3812d61ac3..c4449185ab 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.7.0 (2023-05-02) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index fbb24978b7..20ec4abe17 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Import docs from wiki.ros.org (`#566 `_) +* Contributors: Christoph Fröhlich + 3.7.0 (2023-05-02) ------------------ * Fix JTC from immediately returning success (`#565 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 3b04f3b06f..e19405332b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 3397d446e8..7b682a1de2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 69e4765398..8a82c81953 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index b9c0b090d0..e3e7d12444 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* switch from dash to underscore in setup.cfg (`#595 `_) +* Contributors: Alex Moriarty + 3.7.0 (2023-05-02) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bf0c5a0102..9f9b5924e0 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 4d584c209f..9ef655fb17 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.7.0 (2023-05-02) ------------------ From d510cc1af1923956e8da738012ca6677e20697db Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 14 May 2023 19:49:51 +0100 Subject: [PATCH 266/524] 3.8.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_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 +- 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 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index dd22bd8892..446d92eddc 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 022e8e1353..8246b78bda 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.7.0 + 3.8.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bbab2ab10d..7947ff0f1a 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.8.0 (2023-05-14) +------------------ * Clear registered handles of DiffDriveController on deactivate (`#596 `_) * Contributors: Noel Jiménez García diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0fdbb3a8a5..3fadb63efd 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.7.0 + 3.8.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 63ec09e4c4..167f7c1efb 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 02fc8594c0..5381859c32 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.7.0 + 3.8.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 a17b54b2c0..7fd8390321 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.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f7ab4d61ab..f6bc61e8e7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.7.0 + 3.8.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 b714c9884c..39599caf99 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.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 4441a019ab..76a1731fe7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.7.0 + 3.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 40aa2169ff..4a054fb361 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 4d53c64d7f..0b30330683 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.7.0 + 3.8.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 445341daab..128cc17eda 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.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b5e9e6c9f8..d8a40cc4c9 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.7.0 + 3.8.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 c4449185ab..b3f65276f6 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.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 5ad73b4ea0..4601434879 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.7.0 + 3.8.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 20ec4abe17..fa5824cfe1 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.8.0 (2023-05-14) +------------------ * [JTC] Import docs from wiki.ros.org (`#566 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 00cad650c4..74f2dc6a6e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.7.0 + 3.8.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 e19405332b..cafd1bb4df 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b1ca3b38b7..b2b635ec6c 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.7.0 + 3.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 7b682a1de2..7216b78b1d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b2b7fe286c..b2150411d8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.7.0 + 3.8.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 8a82c81953..0b5de6cc52 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.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 3fee344465..171eb3e6ef 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.7.0 + 3.8.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 3cdf9a76ef..b8be85a628 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.7.0", + version="3.8.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 e3e7d12444..17ad4218ca 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.8.0 (2023-05-14) +------------------ * switch from dash to underscore in setup.cfg (`#595 `_) * Contributors: Alex Moriarty diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 86196cac41..adf8511b0b 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.7.0 + 3.8.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 f04123ec65..0026688db3 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.7.0", + version="3.8.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 9f9b5924e0..04b28102c6 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 83cb39cc7f..b75fc43885 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.7.0 + 3.8.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 9ef655fb17..178077380a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.8.0 (2023-05-14) +------------------ 3.7.0 (2023-05-02) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 4bbf7a2723..5b81d5721f 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.7.0 + 3.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 334e4f2ace819c9912ccd4f5e63c342648b25fc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 May 2023 20:53:50 +0200 Subject: [PATCH 267/524] Fix github links on control.ros.org (#604) * Fix github links * Add local github_url for sphinx --- admittance_controller/doc/userdoc.rst | 4 +++- diff_drive_controller/doc/userdoc.rst | 2 ++ doc/controllers_index.rst | 2 ++ doc/writing_new_controller.rst | 2 ++ effort_controllers/doc/userdoc.rst | 2 ++ force_torque_sensor_broadcaster/doc/userdoc.rst | 2 ++ forward_command_controller/doc/userdoc.rst | 2 ++ imu_sensor_broadcaster/doc/userdoc.rst | 2 ++ joint_state_broadcaster/doc/userdoc.rst | 2 ++ joint_trajectory_controller/doc/userdoc.rst | 2 ++ position_controllers/doc/userdoc.rst | 2 ++ tricycle_controller/doc/userdoc.rst | 2 ++ velocity_controllers/doc/userdoc.rst | 2 ++ 13 files changed, 27 insertions(+), 1 deletion(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index ed2da84ad5..c6a3b91b78 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,6 @@ -.. _addmittance_controller_userdoc: +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/admittance_controller/doc/userdoc.rst + +.. _admittance_controller_userdoc: Admittance Controller ====================== diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 1ef47bd975..25d2383364 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/diff_drive_controller/doc/userdoc.rst + .. _diff_drive_controller_userdoc: diff_drive_controller diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 1c6b1dbd9e..effe814325 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/doc/controllers_index.rst + .. _controllers: ################# diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index dcf577e131..49bd8a11ee 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/doc/writing_new_controller.rst + .. _writing_new_controllers: Writing a new controller diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 000bfcf26c..52539ef795 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/effort_controllers/doc/userdoc.rst + .. _effort_controllers_userdoc: effort_controllers diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index de8a1bd7c6..1d7f1ac1b8 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/force_torque_sensor_broadcaster/doc/userdoc.rst + .. _force_torque_sensor_broadcaster_userdoc: Force Torque Sensor Broadcaster diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 8292bdc1b3..522d671438 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/forward_command_controller/doc/userdoc.rst + .. _forward_command_controller_userdoc: forward_command_controller diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 90704c8504..6f4730b17f 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/imu_sensor_broadcaster/doc/userdoc.rst + .. _imu_sensor_broadcaster_userdoc: IMU Sensor Broadcaster diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index fbdb439992..f809a5b49a 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/joint_state_broadcaster/doc/userdoc.rst + .. _joint_state_broadcaster_userdoc: joint_state_broadcaster diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6494d7116a..63cf0e5b8f 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/joint_trajectory_controller/doc/userdoc.rst + .. _joint_trajectory_controller_userdoc: joint_trajectory_controller diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index 1e5dd53e63..69557209ae 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/position_controllers/doc/userdoc.rst + .. _position_controllers_userdoc: position_controllers diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index a248bbec96..9dd46cc79a 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/tricycle_controller/doc/userdoc.rst + .. _tricycle_controller_userdoc: tricycle_controller diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index 62b6aa019f..56fdde5220 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/velocity_controllers/doc/userdoc.rst + .. _velocity_controllers_userdoc: velocity_controllers From 05d7a5ec73a02eab2cab5943c2d32e95889e73ef Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 16 May 2023 09:03:22 +0200 Subject: [PATCH 268/524] Bump codecov/codecov-action from 3.1.3 to 3.1.4 (#615) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.3 to 3.1.4. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.3...v3.1.4) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6e6e96f032..27528e4e16 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.3 + - uses: codecov/codecov-action@v3.1.4 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 372725edeaba9a77553ee57ce57f8c1d245c837d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 22 May 2023 21:47:47 +0200 Subject: [PATCH 269/524] [JTC] Fix deprecated header (#610) * [JTC] Renovate action tests (#603) * Fix deprecation of qos_event.hpp --- .../src/joint_state_broadcaster.cpp | 2 +- joint_trajectory_controller/CMakeLists.txt | 13 +++++------ .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_actions.cpp | 22 ++++++++----------- .../test/test_trajectory_controller.cpp | 2 +- 5 files changed, 18 insertions(+), 23 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..3c2192d40e 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -24,8 +24,8 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/clock.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rcpputils/split.hpp" diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5ec478d989..615acf0dd7 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -80,13 +80,12 @@ if(BUILD_TESTING) ros2_control_test_assets ) - # TODO(andyz): Disabled due to flakiness - # ament_add_gmock(test_trajectory_actions - # test/test_trajectory_actions.cpp - # ) - # target_link_libraries(test_trajectory_actions - # joint_trajectory_controller - # ) + ament_add_gmock(test_trajectory_actions + test/test_trajectory_actions.cpp + ) + target_link_libraries(test_trajectory_actions + joint_trajectory_controller + ) endif() diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 07080bb1fc..71ed051f66 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -31,10 +31,10 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 10dd3a6b90..ef0a11d961 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -70,17 +70,13 @@ class TestTrajectoryActions : public TrajectoryControllerTest { setup_executor_ = true; - executor_ = std::make_unique(); - - SetUpAndActivateTrajectoryController(true, parameters); - - executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpAndActivateTrajectoryController(executor_, true, parameters); SetUpActionClient(); - executor_->add_node(node_->get_node_base_interface()); + executor_.add_node(node_->get_node_base_interface()); - executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_->spin(); }); + executor_future_handle_ = std::async(std::launch::async, [&]() -> void { executor_.spin(); }); } void SetUpControllerHardware() @@ -132,7 +128,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest if (setup_executor_) { setup_executor_ = false; - executor_->cancel(); + executor_.cancel(); executor_future_handle_.wait(); } } @@ -169,7 +165,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; bool setup_executor_ = false; - rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_; + rclcpp::executors::MultiThreadedExecutor executor_; std::future executor_future_handle_; bool setup_controller_hw_ = false; @@ -417,7 +413,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) common_action_result_code_); // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + updateController(rclcpp::Duration::from_seconds(0.01)); EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); @@ -467,7 +463,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) common_action_result_code_); // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + updateController(rclcpp::Duration::from_seconds(0.01)); EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); @@ -514,7 +510,7 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) common_action_result_code_); // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + updateController(rclcpp::Duration::from_seconds(0.01)); EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); @@ -563,7 +559,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) const double prev_pos3 = joint_pos_[2]; // run an update, it should be holding - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + updateController(rclcpp::Duration::from_seconds(0.01)); EXPECT_EQ(prev_pos1, joint_pos_[0]); EXPECT_EQ(prev_pos2, joint_pos_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e8073086dc..74c17dd03d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -35,13 +35,13 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" From 70e282f7434cf2e3683c233fe8080720c96b3cf2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 25 May 2023 19:46:43 +0200 Subject: [PATCH 270/524] Fix compilation warnings (#621) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * catch exception by reference * Enforce internal linkage for classes in test_gripper_controllers.hpp To avoid problems with HW_IF_POSITION (-Wsubobject-linkage) --------- Co-authored-by: Mathias Lüdtke --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- gripper_controllers/test/test_gripper_controllers.hpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index fa12bd8105..abefd4307f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -194,7 +194,7 @@ controller_interface::return_type DiffDriveController::update( should_publish = true; } } - catch (const std::runtime_error) + catch (const std::runtime_error &) { // Handle exceptions when the time source changes and initialize publish timestamp previous_publish_timestamp_ = time; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..350e551cb8 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -30,6 +30,9 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::StateInterface; +namespace +{ + // subclassing and friending so we can access member variables class FriendGripperController : public gripper_action_controller::GripperActionController @@ -62,4 +65,6 @@ class GripperControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; }; +} // anonymous namespace + #endif // TEST_GRIPPER_CONTROLLERS_HPP_ From 45d008321fe593db0e7f68921baa326b25c38daa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Fri, 26 May 2023 23:34:42 +0200 Subject: [PATCH 271/524] Steering odometry library and controllers (#484) --- .pre-commit-config.yaml | 2 +- ackermann_steering_controller/CMakeLists.txt | 107 ++++ .../ackermann_steering_controller.xml | 8 + ackermann_steering_controller/doc/userdoc.rst | 18 + .../ackermann_steering_controller.hpp | 65 ++ .../visibility_control.h | 52 ++ ackermann_steering_controller/package.xml | 36 ++ .../src/ackermann_steering_controller.cpp | 98 +++ .../src/ackermann_steering_controller.yaml | 40 ++ .../ackermann_steering_controller_params.yaml | 17 + ...steering_controller_preceeding_params.yaml | 17 + .../test_ackermann_steering_controller.cpp | 292 +++++++++ .../test_ackermann_steering_controller.hpp | 319 ++++++++++ ...kermann_steering_controller_preceeding.cpp | 107 ++++ ...est_load_ackermann_steering_controller.cpp | 49 ++ bicycle_steering_controller/CMakeLists.txt | 105 ++++ .../bicycle_steering_controller.xml | 8 + bicycle_steering_controller/doc/userdoc.rst | 19 + .../bicycle_steering_controller.hpp | 61 ++ .../visibility_control.h | 52 ++ bicycle_steering_controller/package.xml | 36 ++ .../src/bicycle_steering_controller.cpp | 87 +++ .../src/bicycle_steering_controller.yaml | 24 + .../bicycle_steering_controller_params.yaml | 15 + ...steering_controller_preceeding_params.yaml | 16 + .../test/test_bicycle_steering_controller.cpp | 266 +++++++++ .../test/test_bicycle_steering_controller.hpp | 284 +++++++++ ...bicycle_steering_controller_preceeding.cpp | 95 +++ .../test_load_bicycle_steering_controller.cpp | 48 ++ doc/controllers_index.rst | 5 + ros2_controllers/package.xml | 4 + steering_controllers_library/CMakeLists.txt | 88 +++ steering_controllers_library/doc/userdoc.rst | 90 +++ .../steering_controllers_library.hpp | 151 +++++ .../steering_odometry.hpp | 261 ++++++++ .../visibility_control.h | 50 ++ steering_controllers_library/package.xml | 45 ++ .../src/steering_controllers_library.cpp | 558 ++++++++++++++++++ .../src/steering_controllers_library.yaml | 122 ++++ .../src/steering_odometry.cpp | 333 +++++++++++ .../steering_controllers_library_params.yaml | 17 + .../test_steering_controllers_library.cpp | 200 +++++++ .../test_steering_controllers_library.hpp | 341 +++++++++++ tricycle_steering_controller/CMakeLists.txt | 105 ++++ tricycle_steering_controller/doc/userdocs.rst | 18 + .../tricycle_steering_controller.hpp | 63 ++ .../visibility_control.h | 52 ++ tricycle_steering_controller/package.xml | 38 ++ .../src/tricycle_steering_controller.cpp | 93 +++ .../src/tricycle_steering_controller.yaml | 32 + ...test_load_tricycle_steering_controller.cpp | 49 ++ .../test_tricycle_steering_controller.cpp | 274 +++++++++ .../test_tricycle_steering_controller.hpp | 302 ++++++++++ ...ricycle_steering_controller_preceeding.cpp | 100 ++++ .../tricycle_steering_controller_params.yaml | 16 + ...steering_controller_preceeding_params.yaml | 16 + .../tricycle_steering_controller.xml | 8 + 57 files changed, 5773 insertions(+), 1 deletion(-) create mode 100644 ackermann_steering_controller/CMakeLists.txt create mode 100644 ackermann_steering_controller/ackermann_steering_controller.xml create mode 100644 ackermann_steering_controller/doc/userdoc.rst create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h create mode 100644 ackermann_steering_controller/package.xml create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_params.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp create mode 100644 ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp create mode 100644 bicycle_steering_controller/CMakeLists.txt create mode 100644 bicycle_steering_controller/bicycle_steering_controller.xml create mode 100644 bicycle_steering_controller/doc/userdoc.rst create mode 100644 bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp create mode 100644 bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h create mode 100644 bicycle_steering_controller/package.xml create mode 100644 bicycle_steering_controller/src/bicycle_steering_controller.cpp create mode 100644 bicycle_steering_controller/src/bicycle_steering_controller.yaml create mode 100644 bicycle_steering_controller/test/bicycle_steering_controller_params.yaml create mode 100644 bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller.cpp create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller.hpp create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp create mode 100644 bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp create mode 100644 steering_controllers_library/CMakeLists.txt create mode 100644 steering_controllers_library/doc/userdoc.rst create mode 100644 steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp create mode 100644 steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp create mode 100644 steering_controllers_library/include/steering_controllers_library/visibility_control.h create mode 100644 steering_controllers_library/package.xml create mode 100644 steering_controllers_library/src/steering_controllers_library.cpp create mode 100644 steering_controllers_library/src/steering_controllers_library.yaml create mode 100644 steering_controllers_library/src/steering_odometry.cpp create mode 100644 steering_controllers_library/test/steering_controllers_library_params.yaml create mode 100644 steering_controllers_library/test/test_steering_controllers_library.cpp create mode 100644 steering_controllers_library/test/test_steering_controllers_library.hpp create mode 100644 tricycle_steering_controller/CMakeLists.txt create mode 100644 tricycle_steering_controller/doc/userdocs.rst create mode 100644 tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp create mode 100644 tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h create mode 100644 tricycle_steering_controller/package.xml create mode 100644 tricycle_steering_controller/src/tricycle_steering_controller.cpp create mode 100644 tricycle_steering_controller/src/tricycle_steering_controller.yaml create mode 100644 tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller.cpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller.hpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp create mode 100644 tricycle_steering_controller/test/tricycle_steering_controller_params.yaml create mode 100644 tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml create mode 100644 tricycle_steering_controller/tricycle_steering_controller.xml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b927aeac78..5a7ac74d9b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -126,7 +126,7 @@ repos: exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.9.0 + rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..ff76da8853 --- /dev/null +++ b/ackermann_steering_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.16) +project(ackermann_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(ackermann_steering_controller_parameters + src/ackermann_steering_controller.yaml +) + +add_library( + ackermann_steering_controller + SHARED + src/ackermann_steering_controller.cpp +) +target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17) +target_include_directories(ackermann_steering_controller PUBLIC + $ + $) +target_link_libraries(ackermann_steering_controller PUBLIC + ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller + test/test_load_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_ackermann_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_ackermann_steering_controller + test/test_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/ackermann_steering_controller +) + +install( + TARGETS ackermann_steering_controller ackermann_steering_controller_parameters + EXPORT export_ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..2ac2150dd1 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints. + + + diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..59cdb78108 --- /dev/null +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -0,0 +1,18 @@ +.. _ackermann_steering_controller_userdoc: + +ackermann_steering_controller +============================= + +This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..0cb6bcd016 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include + +#include "ackermann_steering_controller/visibility_control.h" +#include "ackermann_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + AckermannSteeringController(); + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +protected: + std::shared_ptr ackermann_param_listener_; + ackermann_steering_controller::Params ackermann_params_; +}; +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h new file mode 100644 index 0000000000..177f0bf87c --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml new file mode 100644 index 0000000000..24c3c8406b --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + ackermann_steering_controller + 0.0.0 + Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..c3a7539c5a --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void AckermannSteeringController::initialize_implementation_parameter_listener() +{ + ackermann_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() +{ + ackermann_params_ = ackermann_param_listener_->get_params(); + + const double front_wheels_radius = ackermann_params_.front_wheels_radius; + const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; + const double front_wheel_track = ackermann_params_.front_wheel_track; + const double rear_wheel_track = ackermann_params_.rear_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double front_right_steer_position = + state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..3726146919 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -0,0 +1,40 @@ +ackermann_steering_controller: + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + rear_wheel_track: + { + type: double, + default_value: 0.0, + description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml new file mode 100644 index 0000000000..6b64f901c3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ecb1b071e3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint] + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..480e90e166 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,292 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(AckermannSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(AckermannSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AckermannSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR( + + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + EXPECT_EQ(msg.steering_angle_command[1], 4.4); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp new file mode 100644 index 0000000000..80258258c2 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,319 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; +using ackermann_steering_controller::STATE_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using ackermann_steering_controller::CMD_STEER_LEFT_WHEEL; +using ackermann_steering_controller::CMD_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace + +// subclassing and friending so we can access member variables +class TestableAckermannSteeringController +: public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, update_success); + FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class AckermannSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..2d951588c5 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..0a8cd7b80c --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAckermannSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..8ac12fe0a0 --- /dev/null +++ b/bicycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(bicycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(bicycle_steering_controller_parameters + src/bicycle_steering_controller.yaml +) + +add_library( + bicycle_steering_controller + SHARED + src/bicycle_steering_controller.cpp +) +target_compile_features(bicycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(bicycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(bicycle_steering_controller PUBLIC + bicycle_steering_controller_parameters) +ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(bicycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface bicycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller + test/test_load_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_bicycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller test/test_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml) + target_include_directories(test_bicycle_steering_controller PRIVATE include) + target_link_libraries(test_bicycle_steering_controller bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/bicycle_steering_controller +) + +install( + TARGETS bicycle_steering_controller bicycle_steering_controller_parameters + EXPORT export_bicycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_bicycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/bicycle_steering_controller/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml new file mode 100644 index 0000000000..644c8840fa --- /dev/null +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Bicycle kinematics with one traction and one steering joint. + + + diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..6815dc6953 --- /dev/null +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +.. _bicycle_steering_controller_userdoc: + +bicycle_steering_controller +============================= + +This controller implements the kinematics with two axes and two wheels, where the wheel on one axis is fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have one commanding joint for traction, and one commanding joint for steering. +If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp new file mode 100644 index 0000000000..1b3e050a37 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ +#define BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "bicycle_steering_controller/visibility_control.h" +#include "bicycle_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace bicycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_WHEEL = 0; +static constexpr size_t STATE_STEER_AXIS = 1; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_WHEEL = 0; +static constexpr size_t CMD_STEER_WHEEL = 1; + +static constexpr size_t NR_STATE_ITFS = 2; +static constexpr size_t NR_CMD_ITFS = 2; +static constexpr size_t NR_REF_ITFS = 2; + +class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + BicycleSteeringController(); + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr bicycle_param_listener_; + bicycle_steering_controller::Params bicycle_params_; +}; +} // namespace bicycle_steering_controller + +#endif // BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..b076a00215 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml new file mode 100644 index 0000000000..80932f7fda --- /dev/null +++ b/bicycle_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + bicycle_steering_controller + 0.0.0 + Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp new file mode 100644 index 0000000000..5f013d7d7a --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" + +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void BicycleSteeringController::initialize_implementation_parameter_listener() +{ + bicycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + bicycle_params_ = bicycle_param_listener_->get_params(); + + const double wheelbase = bicycle_params_.wheelbase; + const double front_wheel_radius = bicycle_params_.front_wheel_radius; + const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + } + else + { + odometry_.set_wheel_params(front_wheel_radius, wheelbase); + } + + odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "bicycle odometry configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace bicycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml new file mode 100644 index 0000000000..c40e27ef96 --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -0,0 +1,24 @@ +bicycle_steering_controller: + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Front wheel radius.", + read_only: false, + } + + rear_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheel radius.", + read_only: false, + } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml new file mode 100644 index 0000000000..a2a6c6508b --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -0,0 +1,15 @@ +test_bicycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..39ffeed878 --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_bicycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..06b0c7e846 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -0,0 +1,266 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(BicycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(BicycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(BicycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); +} + +TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[0], 1.1); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp new file mode 100644 index 0000000000..065f9e1a0d --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -0,0 +1,284 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using bicycle_steering_controller::STATE_STEER_AXIS; +using bicycle_steering_controller::STATE_TRACTION_WHEEL; + +// name constants for command interfaces +using bicycle_steering_controller::CMD_STEER_WHEEL; +using bicycle_steering_controller::CMD_TRACTION_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; + +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableBicycleSteeringController +: public bicycle_steering_controller::BicycleSteeringController +{ + FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, activate_success); + FRIEND_TEST(BicycleSteeringControllerTest, update_success); + FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(BicycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(BicycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return bicycle_steering_controller::BicycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class BicycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on max_sub_check_loop_count + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {3.3, 0.5}; + std::array joint_command_values_ = {1.1, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..875910ba23 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..955feb33c5 --- /dev/null +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadBicycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index effe814325..cb5528bdb0 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -42,14 +42,19 @@ Available Controllers .. toctree:: :titlesonly: + Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> Admittance Controller <../admittance_controller/doc/userdoc.rst> + Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Effort Controllers <../effort_controllers/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Available Broadcasters diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b2150411d8..ffdcc89d1a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -10,7 +10,9 @@ ament_cmake + ackermann_steering_controller admittance_controller + bicycle_steering_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster @@ -19,7 +21,9 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt new file mode 100644 index 0000000000..5fdd727188 --- /dev/null +++ b/steering_controllers_library/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.16) +project(steering_controllers_library LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(steering_controllers_library_parameters + src/steering_controllers_library.yaml +) + +add_library( + steering_controllers_library + SHARED + src/steering_controllers_library.cpp + src/steering_odometry.cpp +) +target_compile_features(steering_controllers_library PUBLIC cxx_std_17) +target_include_directories(steering_controllers_library PUBLIC + "$" + "$") +target_link_libraries(steering_controllers_library PUBLIC + steering_controllers_library_parameters) +ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_steering_controllers_library test/test_steering_controllers_library.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_params.yaml) + target_include_directories(test_steering_controllers_library PRIVATE include) + target_link_libraries(test_steering_controllers_library steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_library + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/steering_controllers_library +) + +install( + TARGETS steering_controllers_library steering_controllers_library_parameters + EXPORT export_steering_controllers_library + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_steering_controllers_library HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst new file mode 100644 index 0000000000..27133e2e77 --- /dev/null +++ b/steering_controllers_library/doc/userdoc.rst @@ -0,0 +1,90 @@ +.. _steering_controllers_library_userdoc: + +steering_controllers_library +============================= + +Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). +The library implements generic odometry and update methods and defines the main interfaces. + +Nomenclature used for the controller is used from `wikipedia `_. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x`` and angular ``z`` components are used. +Angular component under +Values in other components are ignored. +In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + +* support for front and rear steering configurations; +* odometry publishing as Odometry and TF message; +* input command timeout based on a parameter. + +The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. +Currently implemented kinematics in corresponding packages are: + +* :ref:`Bicycle ` - with one steering and one drive joints; +* :ref:`Tricylce ` - with one steering and two drive joints; +* :ref:`Ackermann ` - with two seering and two drive joints. + + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- /linear/velocity [double], in m/s +- /angular/position [double] # in [rad] + +Commands +,,,,,,,,, +``front_steering == true`` + +- /position [double] # in [rad] +- /velocity [double] # in [m/s] + +``front_steering == false`` + +- /velocity [double] # in [m/s] +- /position [double] # in [rad] + +States +,,,,,,, +Depending on the ``position_feedback``, different feedback types are expected + +* ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` +* ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` + +``front_steering == true`` + +- /position [double] # in [rad] +- / [double] # in [m] or [m/s] + +``front_steering == false`` + +- / [double] # [m] or [m/s] +- /position [double] # in [rad] + +Subscribers +,,,,,,,,,,,, +Used when controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + If parameter ``use_stamped_vel`` is ``true``. +- /reference_unstamped [geometry_msgs/msg/Twist] + If parameter ``use_stamped_vel`` is ``false``. + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/SteeringControllerStatus] + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp new file mode 100644 index 0000000000..b560e2a782 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -0,0 +1,151 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" +#include "steering_controllers_library/steering_odometry.hpp" +#include "steering_controllers_library/visibility_control.h" +#include "steering_controllers_library_parameters.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "control_msgs/msg/steering_controller_status.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace steering_controllers_library +{ +class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface +{ +public: + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() = 0; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; + +protected: + controller_interface::CallbackReturn set_interface_numbers( + size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + + std::shared_ptr param_listener_; + steering_controllers_library::Params params_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + /// Odometry: + steering_odometry::SteeringOdometry odometry_; + + AckermanControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // name constants for state interfaces + size_t nr_state_itfs_; + // name constants for command interfaces + size_t nr_cmd_itfs_; + // name constants for reference interfaces + size_t nr_ref_itfs_; + + // store last velocity + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; + + std::vector rear_wheels_state_names_; + std::vector front_wheels_state_names_; + +private: + // callback for topic interface + STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( + const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); +}; + +} // namespace steering_controllers_library + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp new file mode 100644 index 0000000000..002db32354 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,261 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ + +#include +#include + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#include +#include "rcpputils/rolling_mean_accumulator.hpp" + +namespace steering_odometry +{ +const unsigned int BICYCLE_CONFIG = 0; +const unsigned int TRICYCLE_CONFIG = 1; +const unsigned int ACKERMANN_CONFIG = 2; +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class SteeringOdometry +{ +public: + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void update_open_loop(const double linear, const double angular, const double dt); + + /** + * \brief Set odometry type + * \param type odometry type + */ + void set_odometry_type(const unsigned int type); + + /** + * \brief heading getter + * \return heading [rad] + */ + double get_heading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double get_x() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double get_y() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double get_linear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double get_angular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ + void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + + /** + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param Vx Desired linear velocity [m/s] + * \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); + + /** + * \brief Reset poses, heading, and accumulators + */ + void reset_odometry(); + +private: + /** + * \brief Uses precomputed linear and angular velocities to compute dometry and update accumulators + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by previous odometry method + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by previous odometry method + */ + bool update_odometry(const double linear_velocity, const double angular, const double dt); + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrate_runge_kutta_2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrate_exact(double linear, double angular); + + /** + * \brief Calculates steering angle from the desired translational and rotational velocity + * \param Vx Linear velocity [m] + * \param theta_dot Angular velocity [rad] + */ + double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + + /** + * \brief Reset linear and angular accumulators + */ + void reset_accumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Kinematic parameters + double wheel_track_; // [m] + double wheelbase_; // [m] + double wheel_radius_; // [m] + + /// Configuration type used for the forward kinematics + int config_type_ = -1; + + /// Previous wheel position/state [rad]: + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; + /// Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + rcpputils::RollingMeanAccumulator linear_acc_; + rcpputils::RollingMeanAccumulator angular_acc_; +}; +} // namespace steering_odometry + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h new file mode 100644 index 0000000000..123662031b --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ +#define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_PROTECTED __attribute__((visibility("protected"))) +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml new file mode 100644 index 0000000000..0d9aa9da39 --- /dev/null +++ b/steering_controllers_library/package.xml @@ -0,0 +1,45 @@ + + + + steering_controllers_library + 0.0.0 + Package for steering robot configurations including odometry and interfaces. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp new file mode 100644 index 0000000000..af2736a8a3 --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers_library/steering_controllers_library.hpp" + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerTwistReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace steering_controllers_library +{ +SteeringControllersLibrary::SteeringControllersLibrary() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + initialize_implementation_parameter_listener(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::set_interface_numbers( + size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) +{ + nr_state_itfs_ = nr_state_itfs; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + configure_odometry(); + + if (!params_.rear_wheels_state_names.empty()) + { + rear_wheels_state_names_ = params_.rear_wheels_state_names; + } + else + { + rear_wheels_state_names_ = params_.rear_wheels_names; + } + + if (!params_.front_wheels_state_names.empty()) + { + front_wheels_state_names_ = params_.front_wheels_state_names; + } + else + { + front_wheels_state_names_ = params_.front_wheels_names; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + if (params_.use_stamped_vel) + { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); + } + + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void SteeringControllersLibrary::reference_callback( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void SteeringControllersLibrary::reference_callback_unstamped( + const std::shared_ptr msg) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " + "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " + "future."); + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(nr_cmd_itfs_); + + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(nr_state_itfs_); + const auto traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + if (params_.front_steering) + { + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + + return state_interfaces_config; +} + +std::vector +SteeringControllersLibrary::on_export_reference_interfaces() +{ + reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs_); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + &reference_interfaces_[1])); + + return reference_interfaces; +} + +bool SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (size_t i = 0; i < nr_cmd_itfs_; ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + else + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type SteeringControllersLibrary::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + update_odometry(period); + + // MOVE ROBOT + + // Limit velocities and accelerations: + // TODO(destogl): add limiter for the velocities + + 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]; + auto [traction_commands, steering_commands] = + odometry_.get_commands(linear_command, angular_command); + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + } + } + else + { + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); + } + } + } + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.get_x(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.get_y(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.traction_wheels_position.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.clear(); + controller_state_publisher_->msg_.linear_velocity_command.clear(); + controller_state_publisher_->msg_.steer_positions.clear(); + controller_state_publisher_->msg_.steering_angle_command.clear(); + + auto number_of_traction_wheels = params_.rear_wheels_names.size(); + auto number_of_steering_wheels = params_.front_wheels_names.size(); + + if (!params_.front_steering) + { + number_of_traction_wheels = params_.front_wheels_names.size(); + number_of_steering_wheels = params_.rear_wheels_names.size(); + } + + for (size_t i = 0; i < number_of_traction_wheels; ++i) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + state_interfaces_[i].get_value()); + } + controller_state_publisher_->msg_.linear_velocity_command.push_back( + command_interfaces_[i].get_value()); + } + + for (size_t i = 0; i < number_of_steering_wheels; ++i) + { + controller_state_publisher_->msg_.steer_positions.push_back( + state_interfaces_[number_of_traction_wheels + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.push_back( + command_interfaces_[number_of_traction_wheels + i].get_value()); + } + + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml new file mode 100644 index 0000000000..86acb01dae --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -0,0 +1,122 @@ +steering_controllers_library: + reference_timeout: { + type: double, + default_value: 1, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + front_steering: { + type: bool, + default_value: true, + description: "Is the steering on the front of the robot?", + read_only: true, + } + + rear_wheels_names: { + type: string_array, + description: "Names of rear wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + front_wheels_names: { + type: string_array, + description: "Names of front wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + rear_wheels_state_names: { + type: string_array, + description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + front_wheels_state_names: { + type: string_array, + description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + open_loop: { + type: bool, + default_value: false, + description: "bool parameter decides if open oop or not (feedback).", + read_only: false, + } + + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + } + + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled ?.", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of pose covariance matrix.", + read_only: false, + } + + position_feedback: { + type: bool, + default_value: false, + description: "Choice of feedback type, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if + position_feedback is true then HW_IF_POSITION is taken as interface type", + read_only: false, + } + + use_stamped_vel: { + type: bool, + default_value: false, + description: "Choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if + use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + read_only: false, + } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp new file mode 100644 index 0000000000..28cd7fc80d --- /dev/null +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -0,0 +1,333 @@ +/********************************************************************* +* Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. + *********************************************************************/ + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#include "steering_controllers_library/steering_odometry.hpp" + +#include +#include + +namespace steering_odometry +{ +SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_track_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + traction_wheel_old_pos_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_acc_(velocity_rolling_window_size), + angular_acc_(velocity_rolling_window_size) +{ +} + +void SteeringOdometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + reset_accumulators(); + timestamp_ = time; +} + +bool SteeringOdometry::update_odometry( + const double linear_velocity, const double angular, const double dt) +{ + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular / dt); + + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); + + return true; +} + +bool SteeringOdometry::update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) +{ + /// Get current wheel joint positions: + const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; + const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; + + /// Update old position with current: + traction_wheel_old_pos_ = traction_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = traction_wheel_est_pos_diff / dt; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double steer_pos, const double dt) +{ + /// Get current wheel joint positions: + const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; + const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; + + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + + /// Update old position with current: + traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; + + const double linear_velocity = + (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + /// Get current wheel joint positions: + const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; + const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; + + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + + /// Update old position with current: + traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = + (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; + steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) +{ + steer_pos_ = steer_pos; + double linear_velocity = traction_wheel_vel * wheel_radius_; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) +{ + double linear_velocity = + (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; + steer_pos_ = steer_pos; + + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; + double linear_velocity = + (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; + const double angular = steer_pos_ * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear * dt, angular * dt); +} + +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +{ + wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; + wheel_track_ = wheel_track; +} + +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + reset_accumulators(); +} + +void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } + +double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase_ / Vx); +} + +std::tuple, std::vector> SteeringOdometry::get_commands( + double Vx, double theta_dot) +{ + // desired velocity and steering angle of the middle of traction and steering axis + double Ws, alpha; + + if (Vx == 0 && theta_dot != 0) + { + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + } + else + { + alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); + Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + } + + if (config_type_ == BICYCLE_CONFIG) + { + std::vector traction_commands = {Ws}; + std::vector steering_commands = {alpha}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == TRICYCLE_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + if (fabs(steer_pos_) < 1e-6) + { + traction_commands = {Ws, Ws}; + } + else + { + double turning_radius = wheelbase_ / std::tan(steer_pos_); + double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } + steering_commands = {alpha}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == ACKERMANN_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + if (fabs(steer_pos_) < 1e-6) + { + traction_commands = {Ws, Ws}; + steering_commands = {alpha, alpha}; + } + else + { + double turning_radius = wheelbase_ / std::tan(steer_pos_); + double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + + double numerator = 2 * wheelbase_ * std::sin(alpha); + double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); + double denominator_second_member = wheel_track_ * std::sin(alpha); + + double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + steering_commands = {alpha_r, alpha_l}; + } + return std::make_tuple(traction_commands, steering_commands); + } + else + { + throw std::runtime_error("Config not implemented"); + } +} + +void SteeringOdometry::reset_odometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + reset_accumulators(); +} + +void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +/** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ +void SteeringOdometry::integrate_exact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrate_runge_kutta_2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void SteeringOdometry::reset_accumulators() +{ + linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace steering_odometry diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml new file mode 100644 index 0000000000..d200d34961 --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp new file mode 100644 index 0000000000..81075d1082 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_steering_controllers_library.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; + + std::shared_ptr msg = std::make_shared(); + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + } + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp new file mode 100644 index 0000000000..23ab7b64f2 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -0,0 +1,341 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// NOTE: Testing steering_controllers_library for ackermann vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & period) { return true; } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersLibraryFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_steering_controllers_library") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_steering_controllers_library/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..c604b89452 --- /dev/null +++ b/tricycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(tricycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(tricycle_steering_controller_parameters + src/tricycle_steering_controller.yaml +) + +add_library( + tricycle_steering_controller + SHARED + src/tricycle_steering_controller.cpp +) +target_compile_features(tricycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(tricycle_steering_controller PUBLIC + tricycle_steering_controller_parameters) +ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(tricycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface tricycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller + test/test_load_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_tricycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller test/test_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) + target_include_directories(test_tricycle_steering_controller PRIVATE include) + target_link_libraries(test_tricycle_steering_controller tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/tricycle_steering_controller +) + +install( + TARGETS tricycle_steering_controller tricycle_steering_controller_parameters + EXPORT export_tricycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_tricycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/tricycle_steering_controller/doc/userdocs.rst b/tricycle_steering_controller/doc/userdocs.rst new file mode 100644 index 0000000000..9a6adfca37 --- /dev/null +++ b/tricycle_steering_controller/doc/userdocs.rst @@ -0,0 +1,18 @@ +.. _tricycle_steering_controller_userdoc: + +tricycle_steering_controller +============================= + +This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp new file mode 100644 index 0000000000..607a684df5 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "steering_controllers_library/steering_controllers_library.hpp" +#include "tricycle_steering_controller/visibility_control.h" +#include "tricycle_steering_controller_parameters.hpp" + +namespace tricycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_AXIS = 2; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_WHEEL = 2; + +static constexpr size_t NR_STATE_ITFS = 3; +static constexpr size_t NR_CMD_ITFS = 3; +static constexpr size_t NR_REF_ITFS = 2; + +class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + TricycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr tricycle_param_listener_; + tricycle_steering_controller::Params tricycle_params_; +}; +} // namespace tricycle_steering_controller + +#endif // TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..606b067ad8 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml new file mode 100644 index 0000000000..e911041cd1 --- /dev/null +++ b/tricycle_steering_controller/package.xml @@ -0,0 +1,38 @@ + + + + tricycle_steering_controller + 0.0.0 + Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp new file mode 100644 index 0000000000..f89d78a52c --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -0,0 +1,93 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} +void TricycleSteeringController::initialize_implementation_parameter_listener() +{ + tricycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + tricycle_params_ = tricycle_param_listener_->get_params(); + + const double front_wheels_radius = tricycle_params_.front_wheels_radius; + const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; + const double wheel_track = tricycle_params_.wheel_track; + const double wheelbase = tricycle_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} + +} // namespace tricycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + tricycle_steering_controller::TricycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml new file mode 100644 index 0000000000..1015865fd9 --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -0,0 +1,32 @@ +tricycle_steering_controller: + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..0d04afdf38 --- /dev/null +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..82ba924305 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(TricycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(TricycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(TricycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp new file mode 100644 index 0000000000..422f399ad4 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -0,0 +1,302 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using tricycle_steering_controller::STATE_STEER_AXIS; +using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using tricycle_steering_controller::CMD_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableTricycleSteeringController +: public tricycle_steering_controller::TricycleSteeringController +{ + FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerTest, update_success); + FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class TricycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double wheel_track_ = 1.212121; + + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..dd72332875 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml new file mode 100644 index 0000000000..6bfb87a892 --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ea8b88002e --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml new file mode 100644 index 0000000000..f0d5d85467 --- /dev/null +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Tricycle kinematics with two traction and one steering joint. + + + From aece9740c50266e34f5edf54c76a9f925f6a48f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 27 May 2023 18:55:15 +0200 Subject: [PATCH 272/524] Use branch name substitution for all links (#618) --- admittance_controller/doc/userdoc.rst | 12 ++++++------ diff_drive_controller/doc/userdoc.rst | 6 +++--- doc/controllers_index.rst | 4 ++-- doc/writing_new_controller.rst | 2 +- effort_controllers/doc/userdoc.rst | 2 +- force_torque_sensor_broadcaster/doc/userdoc.rst | 2 +- forward_command_controller/doc/userdoc.rst | 2 +- imu_sensor_broadcaster/doc/userdoc.rst | 2 +- joint_state_broadcaster/doc/userdoc.rst | 2 +- joint_trajectory_controller/doc/userdoc.rst | 4 ++-- position_controllers/doc/userdoc.rst | 2 +- tricycle_controller/doc/userdoc.rst | 2 +- velocity_controllers/doc/userdoc.rst | 2 +- 13 files changed, 22 insertions(+), 22 deletions(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index c6a3b91b78..152ed890dc 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/admittance_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/admittance_controller/doc/userdoc.rst .. _admittance_controller_userdoc: @@ -18,8 +18,8 @@ Parameters ^^^^^^^^^^^ The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +An example parameter file can be found in the `test folder of the controller `_ Topics @@ -44,14 +44,14 @@ The controller has ``position`` and ``velocity`` reference interfaces exported i States ^^^^^^^ The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. If some interface is not provided, the last commanded interface will be used for calculation. -For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. Commands ^^^^^^^^^ The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 25d2383364..6bd682af26 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/diff_drive_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/diff_drive_controller/doc/userdoc.rst .. _diff_drive_controller_userdoc: @@ -72,9 +72,9 @@ Services Parameters ------------ -Check `parameter definition file for details `_. +Check `parameter definition file for details `_. -Note that the documentation on parameters for joint limits can be found in `their header file `_. +Note that the documentation on parameters for joint limits can be found in `their header file `_. Those parameters are: linear.x [JointLimits structure] diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index cb5528bdb0..f4ce929e78 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/doc/controllers_index.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/controllers_index.rst .. _controllers: @@ -22,7 +22,7 @@ The controllers' namespaces are commanding the following command interface types - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - ... -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 49bd8a11ee..a2e26104a7 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/doc/writing_new_controller.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/writing_new_controller.rst .. _writing_new_controllers: diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 52539ef795..74caf63391 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/effort_controllers/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/effort_controllers/doc/userdoc.rst .. _effort_controllers_userdoc: diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index 1d7f1ac1b8..053723e8f0 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/force_torque_sensor_broadcaster/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/force_torque_sensor_broadcaster/doc/userdoc.rst .. _force_torque_sensor_broadcaster_userdoc: diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 522d671438..2ecba6003a 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/forward_command_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/forward_command_controller/doc/userdoc.rst .. _forward_command_controller_userdoc: diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 6f4730b17f..24438c9959 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/imu_sensor_broadcaster/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/imu_sensor_broadcaster/doc/userdoc.rst .. _imu_sensor_broadcaster_userdoc: diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index f809a5b49a..c9164ec723 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/joint_state_broadcaster/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_state_broadcaster/doc/userdoc.rst .. _joint_state_broadcaster_userdoc: diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 63cf0e5b8f..7529a60918 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/joint_trajectory_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/userdoc.rst .. _joint_trajectory_controller_userdoc: @@ -259,7 +259,7 @@ States ,,,,,,,,,,,,,,,,,, The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. Legal combinations of state interfaces are: diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index 69557209ae..f321e8a698 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/position_controllers/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/position_controllers/doc/userdoc.rst .. _position_controllers_userdoc: diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 9dd46cc79a..d153aeacba 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/tricycle_controller/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_controller/doc/userdoc.rst .. _tricycle_controller_userdoc: diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index 56fdde5220..fa7e36062e 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -1,4 +1,4 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/|github_branch|/velocity_controllers/doc/userdoc.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/velocity_controllers/doc/userdoc.rst .. _velocity_controllers_userdoc: From a3f2a16085fa58a56c52ee9520e4b029996ef303 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 28 May 2023 16:05:03 +0200 Subject: [PATCH 273/524] Fix sphinx for steering odometry library/controllers (#626) --- ackermann_steering_controller/doc/userdoc.rst | 4 +++- bicycle_steering_controller/doc/userdoc.rst | 4 +++- doc/controllers_index.rst | 3 +-- steering_controllers_library/doc/userdoc.rst | 2 ++ .../doc/{userdocs.rst => userdoc.rst} | 4 +++- 5 files changed, 12 insertions(+), 5 deletions(-) rename tricycle_steering_controller/doc/{userdocs.rst => userdoc.rst} (83%) diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst index 59cdb78108..c743b5d709 100644 --- a/ackermann_steering_controller/doc/userdoc.rst +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/ackermann_steering_controller/doc/userdoc.rst + .. _ackermann_steering_controller_userdoc: ackermann_steering_controller @@ -7,7 +9,7 @@ This controller implements the kinematics with two axes and four wheels, where t The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst index 6815dc6953..dd6db9ceaa 100644 --- a/bicycle_steering_controller/doc/userdoc.rst +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/bicycle_steering_controller/doc/userdoc.rst + .. _bicycle_steering_controller_userdoc: bicycle_steering_controller @@ -8,7 +10,7 @@ This controller implements the kinematics with two axes and two wheels, where th The controller expects to have one commanding joint for traction, and one commanding joint for steering. If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index f4ce929e78..80e5e4fda2 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -50,11 +50,10 @@ Available Controllers Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> - Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Available Broadcasters diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 27133e2e77..47d9516fd1 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/steering_controllers_library/doc/userdoc.rst + .. _steering_controllers_library_userdoc: steering_controllers_library diff --git a/tricycle_steering_controller/doc/userdocs.rst b/tricycle_steering_controller/doc/userdoc.rst similarity index 83% rename from tricycle_steering_controller/doc/userdocs.rst rename to tricycle_steering_controller/doc/userdoc.rst index 9a6adfca37..bf20dc58c2 100644 --- a/tricycle_steering_controller/doc/userdocs.rst +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_steering_controller/doc/userdoc.rst + .. _tricycle_steering_controller_userdoc: tricycle_steering_controller @@ -7,7 +9,7 @@ This controller implements the kinematics with two axes and three wheels, where The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters From 0cb6f28cb5444d1a2bdef4d65f01bc2bed8ed121 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 28 May 2023 16:47:50 +0200 Subject: [PATCH 274/524] Use generate_parameter_library for all params (#601) --- diff_drive_controller/src/diff_drive_controller.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index abefd4307f..fc8114630e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -302,10 +302,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + 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, @@ -414,7 +413,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message.child_frame_id = controller_namespace + base_frame_id; // limit the publication on the topics /odom and /tf - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_rate_ = params_.publish_rate; publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); // initialize odom values zeros From 7d785034f40e6375d8305c587710976daef98e21 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 28 May 2023 15:56:59 +0100 Subject: [PATCH 275/524] Bring new packages up to version --- ackermann_steering_controller/package.xml | 2 +- bicycle_steering_controller/package.xml | 2 +- steering_controllers_library/package.xml | 2 +- tricycle_steering_controller/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 24c3c8406b..83fc710c22 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 0.0.0 + 3.8.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/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 80932f7fda..95dd240d58 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 0.0.0 + 3.8.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/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 0d9aa9da39..0395c935da 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 0.0.0 + 3.8.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e911041cd1..652546d098 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 0.0.0 + 3.8.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar From 63057baa85d89566a1bd3e711f5ebdf2149e1da3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 28 May 2023 15:59:38 +0100 Subject: [PATCH 276/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 129 ++++++++++++++++++ admittance_controller/CHANGELOG.rst | 6 + bicycle_steering_controller/CHANGELOG.rst | 129 ++++++++++++++++++ diff_drive_controller/CHANGELOG.rst | 8 ++ effort_controllers/CHANGELOG.rst | 6 + force_torque_sensor_broadcaster/CHANGELOG.rst | 6 + forward_command_controller/CHANGELOG.rst | 6 + gripper_controllers/CHANGELOG.rst | 5 + imu_sensor_broadcaster/CHANGELOG.rst | 6 + joint_state_broadcaster/CHANGELOG.rst | 7 + joint_trajectory_controller/CHANGELOG.rst | 7 + position_controllers/CHANGELOG.rst | 6 + ros2_controllers/CHANGELOG.rst | 5 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 129 ++++++++++++++++++ tricycle_controller/CHANGELOG.rst | 6 + tricycle_steering_controller/CHANGELOG.rst | 129 ++++++++++++++++++ velocity_controllers/CHANGELOG.rst | 6 + 19 files changed, 602 insertions(+) create mode 100644 ackermann_steering_controller/CHANGELOG.rst create mode 100644 bicycle_steering_controller/CHANGELOG.rst create mode 100644 steering_controllers_library/CHANGELOG.rst create mode 100644 tricycle_steering_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..afaf8893e5 --- /dev/null +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -0,0 +1,129 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ackermann_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 446d92eddc..dd4292fbe6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..5d175f004f --- /dev/null +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,129 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package bicycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 7947ff0f1a..09dbef17da 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use generate_parameter_library for all params (`#601 `_) +* Use branch name substitution for all links (`#618 `_) +* Fix compilation warnings (`#621 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García, Mathias Lüdtke + 3.8.0 (2023-05-14) ------------------ * Clear registered handles of DiffDriveController on deactivate (`#596 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 167f7c1efb..a5180731bf 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7fd8390321..0821ea65b2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 39599caf99..64cdb55364 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4a054fb361..a1ec654cac 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix compilation warnings (`#621 `_) +* Contributors: Noel Jiménez García, Mathias Lüdtke + 3.8.0 (2023-05-14) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 128cc17eda..34249024d1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index b3f65276f6..166fde3cdb 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* [JTC] Fix deprecated header (`#610 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index fa5824cfe1..6ee1c3eafd 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* [JTC] Fix deprecated header (`#610 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ * [JTC] Import docs from wiki.ros.org (`#566 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index cafd1bb4df..54c96370d3 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 7216b78b1d..a340fa0163 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Steering odometry library and controllers (`#484 `_) +* Contributors: Tomislav Petković + 3.8.0 (2023-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0b5de6cc52..e17316759d 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.8.0 (2023-05-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 17ad4218ca..2c99b05426 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.8.0 (2023-05-14) ------------------ * switch from dash to underscore in setup.cfg (`#595 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst new file mode 100644 index 0000000000..fdfab092f7 --- /dev/null +++ b/steering_controllers_library/CHANGELOG.rst @@ -0,0 +1,129 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package steering_controllers_library +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 04b28102c6..5ebbbb8c88 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..571ad07d52 --- /dev/null +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,129 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 178077380a..79a68f4f19 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + 3.8.0 (2023-05-14) ------------------ From 1d6ca87b38b1c4bb40980923b5f747e17d7fef27 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 28 May 2023 16:00:32 +0100 Subject: [PATCH 277/524] 3.9.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 +- 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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index afaf8893e5..9a47b7d265 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.9.0 (2023-05-28) +------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) * Steering odometry library and controllers (`#484 `_) * Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 83fc710c22..5060e3f4f9 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.8.0 + 3.9.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 dd4292fbe6..fddcfe1f76 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 8246b78bda..48f59db5b6 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.8.0 + 3.9.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 5d175f004f..2784d4af94 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.9.0 (2023-05-28) +------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) * Steering odometry library and controllers (`#484 `_) * Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 95dd240d58..04fe1ce855 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.8.0 + 3.9.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 09dbef17da..897e6c5161 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.9.0 (2023-05-28) +------------------ * Use generate_parameter_library for all params (`#601 `_) * Use branch name substitution for all links (`#618 `_) * Fix compilation warnings (`#621 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 3fadb63efd..3b58854492 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.8.0 + 3.9.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 a5180731bf..ee2c3664f5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 5381859c32..5bf20f3ac5 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.8.0 + 3.9.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 0821ea65b2..2f8129ba55 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.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f6bc61e8e7..503f971b11 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.8.0 + 3.9.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 64cdb55364..c01351a3fd 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.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 76a1731fe7..78bb855476 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.8.0 + 3.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a1ec654cac..df4099b89c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Fix compilation warnings (`#621 `_) * Contributors: Noel Jiménez García, Mathias Lüdtke diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 0b30330683..189ea82978 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.8.0 + 3.9.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 34249024d1..3675cdd017 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.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index d8a40cc4c9..d2c62459f3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.8.0 + 3.9.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 166fde3cdb..ee09457d8b 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.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * [JTC] Fix deprecated header (`#610 `_) * Fix github links on control.ros.org (`#604 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 4601434879..5c6755b678 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.8.0 + 3.9.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 6ee1c3eafd..5c42a5b977 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.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * [JTC] Fix deprecated header (`#610 `_) * Fix github links on control.ros.org (`#604 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 74f2dc6a6e..4647f59b83 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.8.0 + 3.9.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 54c96370d3..1ed079a953 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b2b635ec6c..8b99f3f094 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.8.0 + 3.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a340fa0163..d3ec738b80 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Steering odometry library and controllers (`#484 `_) * Contributors: Tomislav Petković diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ffdcc89d1a..0de09db8a6 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.8.0 + 3.9.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 e17316759d..56b507196d 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.9.0 (2023-05-28) +------------------ 3.8.0 (2023-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 171eb3e6ef..4efd3a7b67 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.8.0 + 3.9.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 b8be85a628..3aa059415d 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.8.0", + version="3.9.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 2c99b05426..fe97238c44 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.9.0 (2023-05-28) +------------------ 3.8.0 (2023-05-14) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index adf8511b0b..51ca570d42 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.8.0 + 3.9.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 0026688db3..1de53a74c8 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.8.0", + version="3.9.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 fdfab092f7..628c3c9722 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.9.0 (2023-05-28) +------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) * Steering odometry library and controllers (`#484 `_) * Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 0395c935da..67461e6708 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.8.0 + 3.9.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 5ebbbb8c88..bd3eb809fb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index b75fc43885..398618c0bb 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.8.0 + 3.9.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 571ad07d52..ff8cda542f 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.9.0 (2023-05-28) +------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) * Steering odometry library and controllers (`#484 `_) * Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 652546d098..f7dc25b420 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.8.0 + 3.9.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 79a68f4f19..e22c823287 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.9.0 (2023-05-28) +------------------ * Use branch name substitution for all links (`#618 `_) * Fix github links on control.ros.org (`#604 `_) * Contributors: Christoph Fröhlich diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5b81d5721f..714ac4366a 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.8.0 + 3.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From b27b8af7ae9722023612958ac345e20fdf188e81 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 May 2023 06:56:10 +0100 Subject: [PATCH 278/524] Bump actions/setup-python from 4.6.0 to 4.6.1 (#632) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index f0993bf717..8f5be014c5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.6.0 + - uses: actions/setup-python@v4.6.1 with: python-version: '3.10' - name: Install system hooks From 13039200fc7433c039ff8bb9a179b46d5b72bc75 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 May 2023 06:58:35 +0100 Subject: [PATCH 279/524] Bump ros-tooling/setup-ros from 0.6.1 to 0.6.2 (#631) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 27528e4e16..735a713719 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index fd3e6b634b..3149bba54d 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 31bc0d4475..375c3c7f40 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From ce60d2f1858331172ea719f9671057d417be6241 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 May 2023 08:16:31 +0200 Subject: [PATCH 280/524] enable ReflowComments to also use ColumnLimit on comments (#625) --- .clang-format | 2 +- .../admittance_controller.hpp | 14 +- .../admittance_controller/admittance_rule.hpp | 55 +++-- .../test/test_diff_drive_controller.cpp | 12 +- .../forward_controllers_base.hpp | 4 +- .../tolerances.hpp | 4 +- .../trajectory.hpp | 35 +-- .../test/test_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller_utils.hpp | 15 +- .../steering_odometry.hpp | 217 +++++++++--------- .../src/steering_odometry.cpp | 34 +-- .../test/test_tricycle_controller.cpp | 12 +- 12 files changed, 212 insertions(+), 196 deletions(-) diff --git a/.clang-format b/.clang-format index ab8d077577..9e92cf780a 100644 --- a/.clang-format +++ b/.clang-format @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e4aef2cf3d..6ff6cdae7a 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -166,20 +166,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt geometry_msgs::msg::Wrench ft_values_; /** - * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values - */ + * @brief Read values from hardware interfaces and set corresponding fields of state_current and + * ft_values + */ void read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint & state_current, geometry_msgs::msg::Wrench & ft_values); /** - * @brief Set fields of state_reference with values from controllers exported position and velocity references - */ + * @brief Set fields of state_reference with values from controllers exported position and + * velocity references + */ void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); /** -* @brief Write values from state_command to claimed hardware interfaces -*/ + * @brief Write values from state_command to claimed hardware interfaces + */ void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..36c027491c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -43,13 +43,17 @@ struct AdmittanceTransforms { // transformation from force torque sensor frame to base link frame at reference joint angles Eigen::Isometry3d ref_base_ft_; - // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + // transformation from force torque sensor frame to base link frame at reference + admittance + // offset joint angles Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + admittance offset joint angles + // transformation from control frame to base link frame at reference + admittance offset joint + // angles Eigen::Isometry3d base_control_; - // transformation from end effector frame to base link frame at reference + admittance offset joint angles + // transformation from end effector frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + // transformation from center of gravity frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_cog_; // transformation from world frame to base link frame Eigen::Isometry3d world_base_; @@ -111,20 +115,20 @@ class AdmittanceRule controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does - * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation - * are calculated without an error - * \param[in] current_joint_state current joint state of the robot - * \param[in] reference_joint_state input joint state reference - * \param[out] success true if no calls to the kinematics interface fail + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If + * the transform does not exist in the kinematics model, then TF will be used for lookup. The + * return value is true if all transformation are calculated without an error \param[in] + * current_joint_state current joint state of the robot \param[in] reference_joint_state input + * joint state reference \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** - * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field - * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent + * Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, + * damping_) are also updated */ void apply_parameters_update(); @@ -136,7 +140,8 @@ class AdmittanceRule * \param[in] measured_wrench most recent measured wrench from force torque sensor * \param[in] reference_joint_state input joint state reference * \param[in] period time in seconds since last controller update - * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to + * the input reference */ controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -148,7 +153,8 @@ class AdmittanceRule /** * Set fields of `state_message` from current admittance controller state. * - * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + * \param[out] state_message message containing target position/vel/accel, wrench, and actual + * robot state, among other things */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); @@ -159,22 +165,21 @@ class AdmittanceRule protected: /** - * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input - * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin - * calls failed. - * \param[in] admittance_state contains all the information needed to calculate the admittance offset - * \param[in] dt controller period + * Calculates the admittance rule from given the robot's current joint angles. The admittance + * controller state input is updated with the new calculated values. A boolean value is returned + * indicating if any of the kinematics plugin calls failed. \param[in] admittance_state contains + * all the information needed to calculate the admittance offset \param[in] dt controller period * \param[out] success true if no calls to the kinematics interface fail */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, - * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. - * The `wrench_world_` estimate includes gravity compensation - * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame - * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement + * `measured_wrench`, the sensor to base frame rotation `sensor_world_rot`, and the center of + * gravity frame to base frame rotation `cog_world_rot`. The `wrench_world_` estimate includes + * gravity compensation \param[in] measured_wrench most recent measured wrench from force torque + * sensor \param[in] sensor_world_rot rotation matrix from world frame to sensor frame \param[in] + * cog_world_rot rotation matrix from world frame to center of gravity frame */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..adf4e79afc 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -48,12 +48,12 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr } /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout - */ + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ bool wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..d60245f328 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -87,8 +87,8 @@ class ForwardControllersBase : public controller_interface::ControllerInterface * * It is expected that error handling of exceptions is done. * - * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are - * allowed, controller_interface::CallbackReturn::ERROR otherwise. + * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and + * their values are allowed, controller_interface::CallbackReturn::ERROR otherwise. */ virtual controller_interface::CallbackReturn read_parameters() = 0; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index de8f060b1a..e540b06e51 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -123,8 +123,8 @@ SegmentTolerances get_segment_tolerances(Params const & params) * \param state_error State error to check. * \param joint_idx Joint index for the state error * \param state_tolerance State tolerance of joint to check \p state_error against. - * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true - * \return True if \p state_error fulfills \p state_tolerance. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT + * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. */ inline bool check_state_tolerance_per_joint( const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7dac4ddbe1..230d0198f7 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -60,26 +60,29 @@ class Trajectory /// containing trajectory. /** * Sampling trajectory at given \p sample_time. - * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively. - * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment. + * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or + * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to + * be reached at the time defined in the segment. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() * - Sampling exactly on a point of the trajectory: - * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator where point is, end_segment_itr = iterator after + * start_segment_itr * - Sampling between points: - * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after + * start_segment_itr * - Sampling after entire trajectory: * start_segment_itr = --end(), end_segment_itr = end() * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() * return false * * \param[in] sample_time Time at which trajectory will be sampled. - * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all. - * \param[out] expected_state Calculated new at \p sample_time. - * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above. - * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at + * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr + * Iterator to the start segment for given \p sample_time. See description above. \param[out] + * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -91,14 +94,16 @@ class Trajectory /** * Do interpolation between 2 states given a time in between their respective timestamps * - * The start and end states need not necessarily be specified all the way to the acceleration level: + * The start and end states need not necessarily be specified all the way to the acceleration + * level: * - If only \b positions are specified, linear interpolation will be used. * - If \b positions and \b velocities are specified, a cubic spline will be used. - * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used. + * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be + * used. * * If start and end states have different specifications - * (eg. start is position-only, end is position-velocity), the lowest common specification will be used - * (position-only in the example). + * (eg. start is position-only, end is position-velocity), the lowest common specification will be + * used (position-only in the example). * * \param[in] time_a Time at which the segment state equals \p state_a. * \param[in] state_a State at \p time_a. @@ -153,9 +158,9 @@ class Trajectory }; /** - * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices. - * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is - * "{2, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". */ template inline std::vector mapping(const T & t1, const T & t2) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 74c17dd03d..b1188eede7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -552,7 +552,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) const double EPS = 1e-6; /** * @brief check if position error of revolute joints are normalized if not configured so -*/ + */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { rclcpp::executors::MultiThreadedExecutor executor; @@ -659,7 +659,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) /** * @brief check if position error of revolute joints are normalized if configured so -*/ + */ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { rclcpp::executors::MultiThreadedExecutor executor; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c85eb59f58..8f5f64d57f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -61,12 +61,12 @@ class TestableJointTrajectoryController } /** - * @brief wait_for_trajectory block until a new JointTrajectory is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout - */ + * @brief wait_for_trajectory block until a new JointTrajectory is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new JointTrajectory msg was received, false if timeout + */ bool wait_for_trajectory( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) @@ -293,7 +293,8 @@ class TrajectoryControllerTest : public ::testing::Test /** * delay_btwn_points - delay between each points * points - vector of trajectories. One point per controlled joint - * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points + * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided + * points */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, 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 002db32354..51cac3c9a9 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -33,197 +33,200 @@ const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; /** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) - */ + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ class SteeringOdometry { public: /** - * \brief Constructor - * Timestamp will get the current time value - * Value will be set to zero - * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean - * - */ + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); /** - * \brief Initialize the odometry - * \param time Current time - */ + * \brief Initialize the odometry + * \param time Current time + */ void init(const rclcpp::Time & time); /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel velocity [rad] - * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel position [rad] - * \param left_traction_wheel_pos Left traction wheel position [rad] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double traction_wheel_vel, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time - */ + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ void update_open_loop(const double linear, const double angular, const double dt); /** - * \brief Set odometry type - * \param type odometry type - */ + * \brief Set odometry type + * \param type odometry type + */ void set_odometry_type(const unsigned int type); /** - * \brief heading getter - * \return heading [rad] - */ + * \brief heading getter + * \return heading [rad] + */ double get_heading() const { return heading_; } /** - * \brief x position getter - * \return x position [m] - */ + * \brief x position getter + * \return x position [m] + */ double get_x() const { return x_; } /** - * \brief y position getter - * \return y position [m] - */ + * \brief y position getter + * \return y position [m] + */ double get_y() const { return y_; } /** - * \brief linear velocity getter - * \return linear velocity [m/s] - */ + * \brief linear velocity getter + * \return linear velocity [m/s] + */ double get_linear() const { return linear_; } /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ double get_angular() const { return angular_; } /** - * \brief Sets the wheel parameters: radius, separation and wheelbase - */ + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); /** - * \brief Velocity rolling window size setter - * \param velocity_rolling_window_size Velocity rolling window size - */ + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); /** - * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] - * \return Tuple of velocity commands and steering commands - */ + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param Vx Desired linear velocity [m/s] + * \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); /** - * \brief Reset poses, heading, and accumulators - */ + * \brief Reset poses, heading, and accumulators + */ void reset_odometry(); private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update accumulators - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by previous odometry method - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by previous odometry method - */ + * \brief Uses precomputed linear and angular velocities to compute dometry and update + * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) + * computed by previous odometry method \param angular Angular velocity [rad] (angular + * displacement, i.e. m/s * dt) computed by previous odometry method + */ bool update_odometry(const double linear_velocity, const double angular, const double dt); /** - * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by + * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed + * by encoders + */ void integrate_runge_kutta_2(double linear, double angular); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by + * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed + * by encoders + */ void integrate_exact(double linear, double angular); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] - */ + * \brief Calculates steering angle from the desired translational and rotational velocity + * \param Vx Linear velocity [m] + * \param theta_dot Angular velocity [rad] + */ double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); /** - * \brief Reset linear and angular accumulators - */ + * \brief Reset linear and angular accumulators + */ void reset_accumulators(); /// Current timestamp: diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 28cd7fc80d..c480dc1253 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -1,17 +1,17 @@ /********************************************************************* -* Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. *********************************************************************/ /* @@ -303,10 +303,10 @@ void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) } /** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ void SteeringOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..1d6edf261a 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -52,12 +52,12 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle } /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout - */ + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ bool wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) From 96320c66f51fe45b2c75db04cb9de96a0cc6716d Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Fri, 2 Jun 2023 12:59:51 -0300 Subject: [PATCH 281/524] [EOL] Drop Foxy (#641) - Foxy now past EOL date of May 2023 Signed-off-by: Alex Moriarty --- .github/ISSUE_TEMPLATE/good-first-issue.md | 4 +-- .github/mergify.yml | 6 ++--- .github/workflows/README.md | 1 - .github/workflows/foxy-abi-compatibility.yml | 20 -------------- .github/workflows/foxy-binary-build-main.yml | 26 ------------------- .../workflows/foxy-binary-build-testing.yml | 26 ------------------- .../workflows/foxy-semi-binary-build-main.yml | 25 ------------------ .../foxy-semi-binary-build-testing.yml | 25 ------------------ .github/workflows/prerelease-check.yml | 2 -- .../reusable-ros-tooling-source-build.yml | 2 +- README.md | 1 - ros2_controllers-not-released.foxy.repos | 6 ----- ros2_controllers.foxy.repos | 13 ---------- 13 files changed, 6 insertions(+), 151 deletions(-) delete mode 100644 .github/workflows/foxy-abi-compatibility.yml delete mode 100644 .github/workflows/foxy-binary-build-main.yml delete mode 100644 .github/workflows/foxy-binary-build-testing.yml delete mode 100644 .github/workflows/foxy-semi-binary-build-main.yml delete mode 100644 .github/workflows/foxy-semi-binary-build-testing.yml delete mode 100644 ros2_controllers-not-released.foxy.repos delete mode 100644 ros2_controllers.foxy.repos diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 6bbf2e957a..95181eceb0 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) @@ -54,7 +54,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) -* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) **Good luck with your first issue!** diff --git a/.github/mergify.yml b/.github/mergify.yml index 87490702f6..49d2acedfa 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,12 +1,12 @@ pull_request_rules: - - name: Backport to foxy at reviewers discretion + - name: Backport to humble at reviewers discretion conditions: - base=master - - "label=backport-foxy" + - "label=backport-humble" actions: backport: branches: - - foxy + - humble - name: Ask to resolve conflict conditions: diff --git a/.github/workflows/README.md b/.github/workflows/README.md index 5789859350..7727495a97 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -4,7 +4,6 @@ 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) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml deleted file mode 100644 index 7ce17effd0..0000000000 --- a/.github/workflows/foxy-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Foxy - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: foxy - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build-main.yml b/.github/workflows/foxy-binary-build-main.yml deleted file mode 100644 index b306d7e44d..0000000000 --- a/.github/workflows/foxy-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - 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: foxy - ros_repo: main - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml deleted file mode 100644 index 260751abea..0000000000 --- a/.github/workflows/foxy-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - 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: foxy - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-main.yml b/.github/workflows/foxy-semi-binary-build-main.yml deleted file mode 100644 index 75c3295124..0000000000 --- a/.github/workflows/foxy-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - 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: foxy - ros_repo: main - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml deleted file mode 100644 index f6d663cc2c..0000000000 --- a/.github/workflows/foxy-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - 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: foxy - ros_repo: testing - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index acbb42e16b..5e7326e510 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,7 +9,6 @@ on: default: 'rolling' type: choice options: - - foxy - humble - rolling branch: @@ -18,7 +17,6 @@ on: default: 'master' type: choice options: - - foxy - humble - master diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 375c3c7f40..4b2e69c06f 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -14,7 +14,7 @@ on: required: true type: string ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, foxy.' + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble.' default: 'master' required: false type: string diff --git a/README.md b/README.md index d7986d34c6..8c8539a02b 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,6 @@ 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) **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) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.foxy.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos deleted file mode 100644 index d9a1c841a5..0000000000 --- a/ros2_controllers.foxy.repos +++ /dev/null @@ -1,13 +0,0 @@ -repositories: - ros-controls/ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: foxy - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel From 77420cb47482c3c580de51e0086622533d82f7d4 Mon Sep 17 00:00:00 2001 From: julescarpentier <44528057+julescarpentier@users.noreply.github.com> Date: Sat, 3 Jun 2023 01:14:33 +0200 Subject: [PATCH 282/524] removed duplicated previous_publish_timestamp_ increment by publish_period_ in diff_drive_controller.cpp (#644) Co-authored-by: Jules CARPENTIER --- diff_drive_controller/src/diff_drive_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index fc8114630e..2b3b2477b6 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -203,8 +203,6 @@ controller_interface::return_type DiffDriveController::update( if (should_publish) { - previous_publish_timestamp_ += publish_period_; - if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; From 73c848bc42e0c3638e140335a041d78b916f7550 Mon Sep 17 00:00:00 2001 From: Alex Moriarty Date: Sun, 4 Jun 2023 08:07:33 -0300 Subject: [PATCH 283/524] [CI] bump action-ros-ci and pass ref for scheduled build (#640) - fixes #619 where scheduled jobs ran with latest commit from default branch instead of the correct branch Signed-off-by: Alex Moriarty --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 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 735a713719..67d4c43375 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 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 4b2e69c06f..daa378297e 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,9 +32,10 @@ jobs: - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ inputs.ros_distro }} + ref: ${{ inputs.ref }} # build all packages listed in the meta package package-name: diff_drive_controller From b47b42df93e264c901ee8293b4db3c4c2e1fcc19 Mon Sep 17 00:00:00 2001 From: gwalck Date: Sun, 4 Jun 2023 17:57:17 +0200 Subject: [PATCH 284/524] Adapted rqt_jtc to newest control_msgs for jtc (#643) --- .../joint_trajectory_controller.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 7e8ec4948e..99f43e125e 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -80,8 +80,8 @@ class JointTrajectoryController(Plugin): the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - - The controller exposes the C{command} and C{state} topics in its - ROS interface. + - The controller exposes the C{command} and C{controller_state} topics + in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. @@ -252,10 +252,10 @@ def _update_jtc_list(self): def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() - def _on_joint_state_change(self, actual_pos): - assert len(actual_pos) == len(self._joint_pos) - for name in actual_pos.keys(): - self._joint_pos[name]["position"] = actual_pos[name] + def _on_joint_state_change(self, current_pos): + assert len(current_pos) == len(self._joint_pos) + for name in current_pos.keys(): + self._joint_pos[name]["position"] = current_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns @@ -289,11 +289,11 @@ def _on_jtc_enabled(self, val): self._speed_scaling_widget.setEnabled(val) if val: - # Widgets send desired position commands to controller + # Widgets send reference position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: - # Controller updates widgets with actual position + # Controller updates widgets with feedback position self._update_cmd_timer.stop() self._update_act_pos_timer.start() @@ -332,7 +332,7 @@ def _load_jtc(self): # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) - state_topic = jtc_ns + "/state" + state_topic = jtc_ns + "/controller_state" cmd_topic = jtc_ns + "/joint_trajectory" self._state_sub = self._node.create_subscription( JointTrajectoryControllerState, state_topic, self._state_cb, 1 @@ -404,12 +404,12 @@ def _unregister_executor(self): self._executor = None def _state_cb(self, msg): - actual_pos = {} + current_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] - joint_pos = msg.actual.positions[i] - actual_pos[joint_name] = joint_pos - self.jointStateChanged.emit(actual_pos) + joint_pos = msg.feedback.positions[i] + current_pos[joint_name] = joint_pos + self.jointStateChanged.emit(current_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]["command"] = val From a910b9d93395f6d8dd20d06d7aa5b95968cbf152 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 4 Jun 2023 19:00:02 +0100 Subject: [PATCH 285/524] Remove unnecessary include (#645) --- .../include/steering_controllers_library/steering_odometry.hpp | 1 - 1 file changed, 1 deletion(-) 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 51cac3c9a9..3cfa056b27 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -24,7 +24,6 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include #include "rcpputils/rolling_mean_accumulator.hpp" namespace steering_odometry From 9958e32146312547f8741ce1050c0b99d2d7358a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 4 Jun 2023 19:03:08 +0100 Subject: [PATCH 286/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 19 files changed, 73 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 9a47b7d265..43b97d734a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index fddcfe1f76..a857a50765 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 2784d4af94..960d0ddac3 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 897e6c5161..5b727ccb91 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota, Jules CARPENTIER + 3.9.0 (2023-05-28) ------------------ * Use generate_parameter_library for all params (`#601 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ee2c3664f5..7a59a19bc7 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 2f8129ba55..cc0517a432 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.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index c01351a3fd..2980fdbb80 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index df4099b89c..837218b797 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Fix compilation warnings (`#621 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3675cdd017..02bdacd02b 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.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ee09457d8b..4dfd19e5e2 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.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5c42a5b977..81a668731b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1ed079a953..98def79a4c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index d3ec738b80..ba8fa08837 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Steering odometry library and controllers (`#484 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 56b507196d..4f518a2848 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index fe97238c44..9de665c74e 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapted rqt_jtc to newest control_msgs for jtc (`#643 `_) +* Contributors: gwalck + 3.9.0 (2023-05-28) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 628c3c9722..d2b56d2b22 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove unnecessary include (`#645 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.9.0 (2023-05-28) ------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bd3eb809fb..0aaf79cc07 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index ff8cda542f..ce09dd7765 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Fix sphinx for steering odometry library/controllers (`#626 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e22c823287..f6d0f96ed6 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.9.0 (2023-05-28) ------------------ * Use branch name substitution for all links (`#618 `_) From 9a6eec1af1816ff255a334ff67b2ade63e509503 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 4 Jun 2023 19:03:58 +0100 Subject: [PATCH 287/524] 3.10.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 43b97d734a..bede1541f2 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 5060e3f4f9..5bd9c96bc1 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.9.0 + 3.10.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a857a50765..03146c20cf 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 48f59db5b6..4d7530cecc 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.9.0 + 3.10.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 960d0ddac3..868e74bb9c 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 04fe1ce855..99023fffb9 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.9.0 + 3.10.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5b727ccb91..30af3cfb36 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.10.0 (2023-06-04) +------------------- * removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Sai Kishor Kothakota, Jules CARPENTIER diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 3b58854492..0875ba94cd 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.9.0 + 3.10.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 7a59a19bc7..d719d701ff 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 5bf20f3ac5..9a59fe42cd 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.9.0 + 3.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index cc0517a432..74a8612a85 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 503f971b11..d517ed204a 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.9.0 + 3.10.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 2980fdbb80..8d581028b6 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.10.0 (2023-06-04) +------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 78bb855476..5c17d80379 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.9.0 + 3.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 837218b797..f136ab4e44 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 189ea82978..cc55d0be39 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.9.0 + 3.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 02bdacd02b..4cdeba5f33 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index d2c62459f3..6a9a53d3a6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.9.0 + 3.10.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 4dfd19e5e2..f94d8a7e13 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 5c6755b678..ea7a5c9a54 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.9.0 + 3.10.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 81a668731b..09ff60ce87 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.10.0 (2023-06-04) +------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 4647f59b83..9166118724 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.9.0 + 3.10.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 98def79a4c..43389c6f1d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8b99f3f094..f2a968e2d9 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.9.0 + 3.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ba8fa08837..76adc2f316 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0de09db8a6..3a09b034e0 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.9.0 + 3.10.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 4f518a2848..e590ac15b8 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 4efd3a7b67..0649b1afce 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.9.0 + 3.10.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 3aa059415d..0b9f5078e3 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.9.0", + version="3.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 9de665c74e..9a90919f3c 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.10.0 (2023-06-04) +------------------- * Adapted rqt_jtc to newest control_msgs for jtc (`#643 `_) * Contributors: gwalck diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 51ca570d42..fd0dd6199b 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.9.0 + 3.10.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 1de53a74c8..4bf28a6f1c 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.9.0", + version="3.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index d2b56d2b22..2a9c549160 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.10.0 (2023-06-04) +------------------- * Remove unnecessary include (`#645 `_) * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 67461e6708..64f078762a 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.9.0 + 3.10.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0aaf79cc07..f1cf482346 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 398618c0bb..7444292e41 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.9.0 + 3.10.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index ce09dd7765..0565dc1548 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.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f7dc25b420..3478ce0e96 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.9.0 + 3.10.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f6d0f96ed6..cb7feca8da 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.0 (2023-06-04) +------------------- 3.9.0 (2023-05-28) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 714ac4366a..c1ab7d16b3 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.9.0 + 3.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From ec4a24b1afd7e8b51d2c3ca9f5c421bd2843e96e Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Jun 2023 12:34:03 +0100 Subject: [PATCH 288/524] Bump uesteibar/reviewer-lottery from 2 to 3 (#654) --- .github/workflows/reviewer_lottery.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 94f3d9bde6..772809825f 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -8,6 +8,6 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: uesteibar/reviewer-lottery@v2 + - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} From d96f662b9830e030ea80e6564952730744c9c594 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Jun 2023 12:46:37 +0100 Subject: [PATCH 289/524] Second round of dependencies fix (#655) * Add missing backward_ros dependency and minor cmake cleanup * Fix unused arg warning --- steering_controllers_library/CMakeLists.txt | 1 - steering_controllers_library/package.xml | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 5fdd727188..4a98dbf320 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -55,7 +55,6 @@ target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTRO if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) add_rostest_with_parameters_gmock( diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 64f078762a..2684a2f727 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -18,6 +18,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface geometry_msgs @@ -36,7 +37,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 23ab7b64f2..c72c61257a 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -138,7 +138,7 @@ class TestableSteeringControllersLibrary return controller_interface::CallbackReturn::SUCCESS; } - bool update_odometry(const rclcpp::Duration & period) { return true; } + bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } private: rclcpp::WaitSet ref_subscriber_wait_set_; From 129057ba50477c78f5883d8c735d5957821b0585 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Jun 2023 12:47:46 +0100 Subject: [PATCH 290/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 19 files changed, 59 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index bede1541f2..6375badb25 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 03146c20cf..57e7dd84ac 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 868e74bb9c..f913267b94 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 30af3cfb36..d33d4c3018 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.10.0 (2023-06-04) ------------------- * removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d719d701ff..29ff565560 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 74a8612a85..f7025b8e60 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.10.0 (2023-06-04) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 8d581028b6..2256b00eb6 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.10.0 (2023-06-04) ------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f136ab4e44..6c7bee11e0 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4cdeba5f33..fb8749605d 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.10.0 (2023-06-04) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f94d8a7e13..f786d648f2 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.10.0 (2023-06-04) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 09ff60ce87..f538df0739 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 43389c6f1d..460d1c16ce 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 76adc2f316..a702904ba9 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e590ac15b8..c92759797c 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 9a90919f3c..f9b0f43a29 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.10.0 (2023-06-04) ------------------- * Adapted rqt_jtc to newest control_msgs for jtc (`#643 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 2a9c549160..b2fa703063 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Second round of dependencies fix (`#655 `_) +* Contributors: Bence Magyar + 3.10.0 (2023-06-04) ------------------- * Remove unnecessary include (`#645 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f1cf482346..e67efd3d4b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- * enable ReflowComments to also use ColumnLimit on comments (`#625 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 0565dc1548..95d95e9ad5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cb7feca8da..79d4bc0a32 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.0 (2023-06-04) ------------------- From d14c70416d0a88db810904d274112414cdcd7a57 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Jun 2023 12:48:23 +0100 Subject: [PATCH 291/524] 3.10.1 --- 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 +- 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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 6375badb25..af5ef64661 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 5bd9c96bc1..0c833147ea 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.10.0 + 3.10.1 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 57e7dd84ac..82bd365880 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 4d7530cecc..4506992846 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.10.0 + 3.10.1 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 f913267b94..6d4921dea9 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 99023fffb9..3a777bd441 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.10.0 + 3.10.1 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 d33d4c3018..2cc4720e5a 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0875ba94cd..669d1067b7 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.10.0 + 3.10.1 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 29ff565560..b30dbb4d6e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 9a59fe42cd..5a9f239d58 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.10.0 + 3.10.1 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 f7025b8e60..8b303c862d 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index d517ed204a..955650fcf9 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.10.0 + 3.10.1 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 2256b00eb6..e7b0a22fcf 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 5c17d80379..f7b744ef61 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.10.0 + 3.10.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6c7bee11e0..1c2e25f72e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index cc55d0be39..06fd59362f 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.10.0 + 3.10.1 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index fb8749605d..3869b5e229 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6a9a53d3a6..a5b4dd06bf 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.10.0 + 3.10.1 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 f786d648f2..050460c0a8 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index ea7a5c9a54..1be245a957 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.10.0 + 3.10.1 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f538df0739..a8f89aea96 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9166118724..f91322b047 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.10.0 + 3.10.1 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 460d1c16ce..4f064e3140 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index f2a968e2d9..2829642bba 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.10.0 + 3.10.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a702904ba9..1c600fbd73 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3a09b034e0..1b7f76ed01 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.10.0 + 3.10.1 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 c92759797c..0688058681 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0649b1afce..71dec82527 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.10.0 + 3.10.1 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 0b9f5078e3..96f08db712 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.10.0", + version="3.10.1", 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 f9b0f43a29..2e9e8325f7 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index fd0dd6199b..4be20ed909 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.10.0 + 3.10.1 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 4bf28a6f1c..146c961401 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.10.0", + version="3.10.1", 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 b2fa703063..a773bb2bcc 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.10.1 (2023-06-06) +------------------- * Second round of dependencies fix (`#655 `_) * Contributors: Bence Magyar diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 2684a2f727..688b84167a 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.10.0 + 3.10.1 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 e67efd3d4b..13b98ea203 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 7444292e41..4098984815 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.10.0 + 3.10.1 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 95d95e9ad5..a20856bd65 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.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 3478ce0e96..e82ef7e202 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.10.0 + 3.10.1 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 79d4bc0a32..0852b7e474 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.10.1 (2023-06-06) +------------------- 3.10.0 (2023-06-04) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index c1ab7d16b3..5d71730f8f 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.10.0 + 3.10.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From c74a3299e3d5f5bf27bddb7021007988e76ea7b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 12 Jun 2023 15:55:15 +0200 Subject: [PATCH 292/524] Let sphinx add parameter description to documentation (#651) --- ackermann_steering_controller/doc/userdoc.rst | 4 +++- bicycle_steering_controller/doc/userdoc.rst | 4 +++- doc/controllers_index.rst | 1 + forward_command_controller/doc/userdoc.rst | 15 +++++++++++++++ gripper_controllers/doc/userdoc.rst | 14 ++++++++++++++ imu_sensor_broadcaster/doc/userdoc.rst | 8 ++------ .../src/imu_sensor_broadcaster_parameters.yaml | 4 +++- steering_controllers_library/doc/userdoc.rst | 5 +++-- tricycle_steering_controller/doc/userdoc.rst | 4 +++- 9 files changed, 47 insertions(+), 12 deletions(-) create mode 100644 gripper_controllers/doc/userdoc.rst diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst index c743b5d709..c4ae35e5b0 100644 --- a/ackermann_steering_controller/doc/userdoc.rst +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -15,6 +15,8 @@ For more details on controller's execution and interfaces check the :ref:`Steeri Parameters ,,,,,,,,,,, -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/ackermann_steering_controller.yaml diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst index dd6db9ceaa..a720ff887d 100644 --- a/bicycle_steering_controller/doc/userdoc.rst +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -16,6 +16,8 @@ For more details on controller's execution and interfaces check the :ref:`Steeri Parameters ,,,,,,,,,,, -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/bicycle_steering_controller.yaml diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 80e5e4fda2..763381c065 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -48,6 +48,7 @@ Available Controllers Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 2ecba6003a..ee0af57d7b 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -15,3 +15,18 @@ Hardware interface type ----------------------- This controller can be used for every type of command interface. + +Parameters +------------ + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +forward_command_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. generate_parameter_library_details:: ../src/forward_command_controller_parameters.yaml + +multi_interface_forward_command_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. generate_parameter_library_details:: ../src/multi_interface_forward_command_controller_parameters.yaml diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..28262e90f9 --- /dev/null +++ b/gripper_controllers/doc/userdoc.rst @@ -0,0 +1,14 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gripper_controllers/doc/userdoc.rst + +.. _gripper_controllers_userdoc: + +Gripper Action Controller +-------------------------------- + +Controller for executing a gripper command action for simple single-dof grippers. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 24438c9959..3b8ae172db 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -11,10 +11,6 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -frame_id (mandatory) - Frame in which the output message will be published. +This controller uses the `generate_parameter_library `_ to handle its parameters. -sensor_name (mandatory) - Defines sensor name used as prefix for its interfaces. - Interface names are: /orientation.x, ..., /angular_velocity.x, ..., - /linear_acceleration.x. +.. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index c0daba9f11..bd63bf4aa6 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -2,7 +2,9 @@ imu_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Defines sensor name used as prefix for its interfaces. + Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., + /linear_acceleration.x.``", } frame_id: { type: string, diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 47d9516fd1..df3d1529d0 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -86,7 +86,8 @@ Publishers Parameters ,,,,,,,,,,, - -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/steering_controllers_library.yaml diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst index bf20dc58c2..d76f489745 100644 --- a/tricycle_steering_controller/doc/userdoc.rst +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -15,6 +15,8 @@ For more details on controller's execution and interfaces check the :ref:`Steeri Parameters ,,,,,,,,,,, -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/tricycle_steering_controller.yaml From c02bc40787fbfb5a9e487022fa6f210cfb95a8f0 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 12 Jun 2023 17:45:56 +0100 Subject: [PATCH 293/524] Updated hyperlink of pluginlib documentation (#647) (#662) --- doc/writing_new_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index a2e26104a7..190b76832b 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -5,7 +5,7 @@ Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** From 23f0defd94a5a80f07c339b0babc736d2d0dcd25 Mon Sep 17 00:00:00 2001 From: gwalck Date: Tue, 13 Jun 2023 20:46:31 +0200 Subject: [PATCH 294/524] Added -Wconversion flag and fix warnings (#667) --- ackermann_steering_controller/CMakeLists.txt | 2 +- admittance_controller/CMakeLists.txt | 2 +- bicycle_steering_controller/CMakeLists.txt | 2 +- diff_drive_controller/CMakeLists.txt | 2 +- diff_drive_controller/src/diff_drive_controller.cpp | 4 ++-- effort_controllers/CMakeLists.txt | 2 +- force_torque_sensor_broadcaster/CMakeLists.txt | 2 +- forward_command_controller/CMakeLists.txt | 2 +- .../test/test_forward_command_controller.cpp | 2 +- ...test_multi_interface_forward_command_controller.cpp | 4 ++-- gripper_controllers/CMakeLists.txt | 2 +- imu_sensor_broadcaster/CMakeLists.txt | 2 +- joint_state_broadcaster/CMakeLists.txt | 2 +- joint_trajectory_controller/CMakeLists.txt | 2 +- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- position_controllers/CMakeLists.txt | 2 +- steering_controllers_library/CMakeLists.txt | 2 +- tricycle_controller/CMakeLists.txt | 2 +- tricycle_controller/src/traction_limiter.cpp | 2 +- tricycle_controller/src/tricycle_controller.cpp | 10 +++++----- tricycle_steering_controller/CMakeLists.txt | 2 +- velocity_controllers/CMakeLists.txt | 2 +- 23 files changed, 29 insertions(+), 29 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index ff76da8853..6ad0e9485f 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() # find dependencies diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 9ece5e7fb0..a8a8832fce 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 8ac12fe0a0..5d662c1fec 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() # find dependencies diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 1077982380..b944ff1e88 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2b3b2477b6..20736fda09 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= params_.wheels_per_side; - right_feedback_mean /= params_.wheels_per_side; + left_feedback_mean /= static_cast(params_.wheels_per_side); + right_feedback_mean /= static_cast(params_.wheels_per_side); if (params_.position_feedback) { diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 340bb19825..ad97690655 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 207e978c10..eaa5cabac6 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 46189fe5ac..15ffe09750 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..b39294f36c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -206,7 +206,7 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check joint commands have been modified 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 0cada04859..c629b36ba8 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 @@ -205,7 +205,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set @@ -289,7 +289,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index e8fd1e5d7e..8edaaf6386 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 18d883503b..991a463284 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 018adef23a..cadc96b4e3 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 615acf0dd7..35223016b7 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index e540b06e51..b4cb029dd9 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -127,7 +127,7 @@ SegmentTolerances get_segment_tolerances(Params const & params) * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. */ inline bool check_state_tolerance_per_joint( - const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, const StateTolerances & state_tolerance, bool show_errors = false) { using std::abs; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 71ed051f66..6941527649 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -117,7 +117,7 @@ controller_interface::return_type JointTrajectoryController::update( } auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, + JointTrajectoryPoint & error, size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) { diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 05714a6e46..54a4f94656 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 4a98dbf320..b64ea204a8 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() # find dependencies diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index ea2232f88f..291f08ad9f 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index b9759e3624..a8019714e1 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -102,7 +102,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) double dv_min; double dv_max; - if (abs(v) >= abs(v0)) + if (std::fabs(v) >= std::fabs(v0)) { dv_min = min_acceleration_ * dt; dv_max = max_acceleration_ * dt; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 4f4e22ec9d..d6aeec0af1 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -67,7 +67,7 @@ CallbackReturn TricycleController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("odom_only_twist", odom_params_.odom_only_twist); - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); @@ -234,8 +234,8 @@ controller_interface::return_type TricycleController::update( AckermannDrive ackermann_command; // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel // speed (rad/s) - ackermann_command.speed = Ws_write; - ackermann_command.steering_angle = alpha_write; + ackermann_command.speed = static_cast(Ws_write); + ackermann_command.steering_angle = static_cast(alpha_write); previous_commands_.emplace(ackermann_command); // Publish ackermann command @@ -244,8 +244,8 @@ controller_interface::return_type TricycleController::update( auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel // speed (rad/s) - realtime_ackermann_command.speed = Ws_write; - realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command.speed = static_cast(Ws_write); + realtime_ackermann_command.steering_angle = static_cast(alpha_write); realtime_ackermann_command_publisher_->unlockAndPublish(); } diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index c604b89452..4c56d68185 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() # find dependencies diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 27ec8417bb..3642ae1cb9 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 2f6176f71feead172bf1f22e603d78b184bb20d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 13 Jun 2023 20:49:49 +0200 Subject: [PATCH 295/524] Use link pointing to docs.ros.org (#666) --- doc/writing_new_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 190b76832b..65ffc6795b 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -5,7 +5,7 @@ Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** From f059022622e12115d006e0677ca2e312eed78858 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 14 Jun 2023 13:45:10 +0200 Subject: [PATCH 296/524] Broadcaster parameters (#650) --- .../CMakeLists.txt | 11 +++-- .../src/force_torque_sensor_broadcaster.cpp | 6 --- ..._torque_sensor_broadcaster_parameters.yaml | 3 ++ ...orce_torque_sensor_broadcaster_params.yaml | 4 ++ .../test_force_torque_sensor_broadcaster.cpp | 36 +++++----------- ...t_load_force_torque_sensor_broadcaster.cpp | 9 +++- imu_sensor_broadcaster/CMakeLists.txt | 11 +++-- .../src/imu_sensor_broadcaster.cpp | 11 ----- .../imu_sensor_broadcaster_parameters.yaml | 6 +++ .../test/imu_sensor_broadcaster_params.yaml | 5 +++ .../test/test_imu_sensor_broadcaster.cpp | 43 +++++-------------- .../test/test_load_imu_sensor_broadcaster.cpp | 9 +++- 12 files changed, 68 insertions(+), 86 deletions(-) create mode 100644 force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml create mode 100644 imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index eaa5cabac6..2af5500e21 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -49,11 +49,13 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) @@ -63,9 +65,10 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster test/test_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9687f9dfee..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -72,12 +72,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return controller_interface::CallbackReturn::ERROR; - } - if (!params_.sensor_name.empty()) { force_torque_sensor_ = std::make_unique( diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 059d5ab9a1..68a85d9d8e 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -3,6 +3,9 @@ force_torque_sensor_broadcaster: type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } sensor_name: { type: string, diff --git a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..58cc1c6b16 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_force_torque_sensor_broadcaster: + ros__parameters: + + frame_id: "fts_sensor_frame" 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 74e2b15da0..b88266712b 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 @@ -39,9 +39,9 @@ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void ForceTorqueSensorBroadcasterTest::SetUpTestCase() {} -void ForceTorqueSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void ForceTorqueSensorBroadcasterTest::TearDownTestCase() {} void ForceTorqueSensorBroadcasterTest::SetUp() { @@ -106,29 +106,6 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) { SetUpFTSBroadcaster(); @@ -315,3 +292,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index a9ca5e88fc..0c269d6a31 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -43,6 +41,13 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) "test_force_torque_sensor_broadcaster", "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 991a463284..3b09d9e72e 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -49,11 +49,13 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_imu_sensor_broadcaster imu_sensor_broadcaster ) @@ -63,9 +65,10 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_imu_sensor_broadcaster test/test_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster ) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 3e2d574f0f..04a28e5368 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -44,17 +44,6 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - if (params_.sensor_name.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); - return CallbackReturn::ERROR; - } - - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return CallbackReturn::ERROR; - } imu_sensor_ = std::make_unique( semantic_components::IMUSensor(params_.sensor_name)); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index bd63bf4aa6..d45bf8583b 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -5,11 +5,17 @@ imu_sensor_broadcaster: description: "Defines sensor name used as prefix for its interfaces. Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., /linear_acceleration.x.``", + validation: { + not_empty<>: null + } } frame_id: { type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } static_covariance_orientation: { type: double_array, diff --git a/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..c1c660d2c4 --- /dev/null +++ b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml @@ -0,0 +1,5 @@ +test_imu_sensor_broadcaster: + ros__parameters: + + sensor_name: "imu_sensor" + frame_id: "imu_sensor_frame" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 31c8d9e2f0..8358088b1d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -40,9 +40,9 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void IMUSensorBroadcasterTest::SetUpTestCase() {} -void IMUSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void IMUSensorBroadcasterTest::TearDownTestCase() {} void IMUSensorBroadcasterTest::SetUp() { @@ -102,36 +102,6 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & ASSERT_TRUE(subscription->take(imu_msg, msg_info)); } -TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // configure failed - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, SensorName_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - // configure failed, 'sensor_name' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'sensor_name' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) { SetUpIMUBroadcaster(); @@ -208,3 +178,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) EXPECT_EQ(imu_msg.linear_acceleration_covariance[i], 0.0); } } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index e7cfd1bcf7..f4e6105ed6 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -42,6 +40,13 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) cm.load_controller( "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } From 6c979091a7e6477963226765a23ecadd33a70d3a Mon Sep 17 00:00:00 2001 From: "G.A. vd. Hoorn" Date: Fri, 16 Jun 2023 23:54:16 +0200 Subject: [PATCH 297/524] jtc: fix minor typo in traj validation error msg (#674) --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6941527649..4d994e26bf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1299,7 +1299,7 @@ bool JointTrajectoryController::validate_trajectory_msg( { RCLCPP_ERROR( get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } From 21e3f2268da78f6157deec88107e394845942049 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 20 Jun 2023 22:39:15 +0200 Subject: [PATCH 298/524] Removes deprecated if-branch (#653) --- .../publisher_joint_trajectory_controller.py | 114 +++++++----------- 1 file changed, 45 insertions(+), 69 deletions(-) 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 56b56c5998..4ed198515e 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 @@ -68,83 +68,59 @@ def __init__(self): self.goals = [] # List of JointTrajectoryPoint for name in goal_names: self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) - goal = self.get_parameter(name).value - # TODO(anyone): remove this "if" part in ROS Iron - if isinstance(goal, list): - self.get_logger().warn( - f'Goal "{name}" is defined as a list. This is deprecated. ' - "Use the following structure:\n:\n " - "positions: [joint1, joint2, joint3, ...]\n " - "velocities: [v_joint1, v_joint2, ...]\n " - "accelerations: [a_joint1, a_joint2, ...]\n " - "effort: [eff_joint1, eff_joint2, ...]" - ) + point = JointTrajectoryPoint() - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') + def get_sub_param(sub_param): + param_name = name + "." + sub_param + self.declare_parameter(param_name, [float()]) + param_value = self.get_parameter(param_name).value - float_goal = [float(value) for value in goal] + float_values = [] - point = JointTrajectoryPoint() - point.positions = float_goal - point.time_from_start = Duration(sec=4) + if len(param_value) != len(self.joints): + return [False, float_values] + + float_values = [float(value) for value in param_value] + + return [True, float_values] + + one_ok = False + + [ok, values] = get_sub_param("positions") + if ok: + point.positions = values + one_ok = True + [ok, values] = get_sub_param("velocities") + if ok: + point.velocities = values + one_ok = True + + [ok, values] = get_sub_param("accelerations") + if ok: + point.accelerations = values + one_ok = True + + [ok, values] = get_sub_param("effort") + if ok: + point.effort = values + one_ok = True + + if one_ok: + point.time_from_start = Duration(sec=4) self.goals.append(point) + self.get_logger().info(f'Goal "{name}" has definition {point}') else: - point = JointTrajectoryPoint() - - def get_sub_param(sub_param): - param_name = name + "." + sub_param - self.declare_parameter(param_name, [float()]) - param_value = self.get_parameter(param_name).value - - float_values = [] - - if len(param_value) != len(self.joints): - return [False, float_values] - - float_values = [float(value) for value in param_value] - - return [True, float_values] - - one_ok = False - - [ok, values] = get_sub_param("positions") - if ok: - point.positions = values - one_ok = True - - [ok, values] = get_sub_param("velocities") - if ok: - point.velocities = values - one_ok = True - - [ok, values] = get_sub_param("accelerations") - if ok: - point.accelerations = values - one_ok = True - - [ok, values] = get_sub_param("effort") - if ok: - point.effort = values - one_ok = True - - if one_ok: - point.time_from_start = Duration(sec=4) - self.goals.append(point) - self.get_logger().info(f'Goal "{name}" has definition {point}') - - else: - self.get_logger().warn( - f'Goal "{name}" definition is wrong. This goal will not be used. ' - "Use the following structure: \n:\n " - "positions: [joint1, joint2, joint3, ...]\n " - "velocities: [v_joint1, v_joint2, ...]\n " - "accelerations: [a_joint1, a_joint2, ...]\n " - "effort: [eff_joint1, eff_joint2, ...]" - ) + self.get_logger().warn( + f'Goal "{name}" definition is wrong. This goal will not be used. ' + "Use the following structure: \n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) if len(self.goals) < 1: self.get_logger().error("No valid goal found. Exiting...") From c619aac5f67cd323129eec6889d814ab1c160c87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 22 Jun 2023 15:19:45 +0200 Subject: [PATCH 299/524] Fix cpplint (#681) --- admittance_controller/test/test_admittance_controller.hpp | 2 +- ..._description.hpp => test_asset_6d_robot_description.hpp} | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) rename admittance_controller/test/{6d_robot_description.hpp => test_asset_6d_robot_description.hpp} (98%) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7dca2e2010..d6c95d2e1c 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -27,7 +27,6 @@ #include "gmock/gmock.h" -#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +37,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" +#include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp similarity index 98% rename from admittance_controller/test/6d_robot_description.hpp rename to admittance_controller/test/test_asset_6d_robot_description.hpp index f67b5bd054..4d38df7c30 100644 --- a/admittance_controller/test/6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #include @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf = } // namespace ros2_control_test_assets -#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ From 38ebec73bf176a4161924db5681073f12c491d0f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Jun 2023 08:13:38 +0100 Subject: [PATCH 300/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 19 files changed, 102 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index af5ef64661..4aba3813d5 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 82bd365880..aaf039ec89 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix cpplint (`#681 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 6d4921dea9..f1c5028d3d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 2cc4720e5a..86de416fe9 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b30dbb4d6e..87b712bf6a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8b303c862d..5265a506d6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Broadcaster parameters (`#650 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e7b0a22fcf..f6add3e14c 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 1c2e25f72e..93fc28bf7f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3869b5e229..00d8b01884 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Broadcaster parameters (`#650 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 050460c0a8..07eef0514a 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a8f89aea96..ec1192edfe 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* jtc: fix minor typo in traj validation error msg (`#674 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: G.A. vd. Hoorn, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 4f064e3140..5484a1efcf 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1c600fbd73..25beb674e5 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.10.1 (2023-06-06) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0688058681..a25db69db8 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 +----------- +* Removes deprecated if-branch (`#653 `_) +* Contributors: Christoph Fröhlich + 3.10.1 (2023-06-06) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 2e9e8325f7..5e50dcb841 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.10.1 (2023-06-06) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index a773bb2bcc..57d4845268 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- * Second round of dependencies fix (`#655 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 13b98ea203..2af274323f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a20856bd65..81ff7aa1e1 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + 3.10.1 (2023-06-06) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 0852b7e474..f766725611 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + 3.10.1 (2023-06-06) ------------------- From cd40ec0e9410b303d94709405be7c4d0e97c3b3b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Jun 2023 08:14:03 +0100 Subject: [PATCH 301/524] 3.11.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 4aba3813d5..6d5626ccd0 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 0c833147ea..132cac06c0 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.10.1 + 3.11.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index aaf039ec89..56feda699a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Fix cpplint (`#681 `_) * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 4506992846..ba6d549877 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.10.1 + 3.11.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index f1c5028d3d..1b7a761aeb 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 3a777bd441..e77fd0e0bb 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.10.1 + 3.11.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 86de416fe9..fddec6b53d 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 669d1067b7..f7d2f2f361 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.10.1 + 3.11.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 87b712bf6a..b21b0c547d 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 5a9f239d58..567dc00741 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.10.1 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 5265a506d6..de8adc57f1 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.11.0 (2023-06-24) +------------------- * Broadcaster parameters (`#650 `_) * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 955650fcf9..a363551958 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.10.1 + 3.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index f6add3e14c..796b01c03f 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f7b744ef61..0069f28315 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.10.1 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 93fc28bf7f..e4cb8cad5e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 06fd59362f..646399f14f 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.10.1 + 3.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 00d8b01884..5252da9c44 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.11.0 (2023-06-24) +------------------- * Broadcaster parameters (`#650 `_) * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index a5b4dd06bf..993bec7ab4 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.10.1 + 3.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 07eef0514a..9337cd24b2 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1be245a957..da7683d69e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.10.1 + 3.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ec1192edfe..354516fbb2 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.11.0 (2023-06-24) +------------------- * jtc: fix minor typo in traj validation error msg (`#674 `_) * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: G.A. vd. Hoorn, gwalck diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f91322b047..edf2ac91a9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.10.1 + 3.11.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 5484a1efcf..299b7b7de7 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2829642bba..d2c4eaecc6 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.10.1 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 25beb674e5..4ca34fc0a5 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- 3.10.1 (2023-06-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 1b7f76ed01..ce06ac9d71 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.10.1 + 3.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a25db69db8..f1a00b9bf4 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.11.0 (2023-06-24) +------------------- * Removes deprecated if-branch (`#653 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 71dec82527..27cc252293 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.10.1 + 3.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 96f08db712..35f7f7b942 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.10.1", + version="3.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5e50dcb841..648b093d2f 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.11.0 (2023-06-24) +------------------- 3.10.1 (2023-06-06) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4be20ed909..c44049b9ff 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.10.1 + 3.11.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 146c961401..5610eff43c 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.10.1", + version="3.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 57d4845268..596f1fe9d3 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 688b84167a..5918c1491e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.10.1 + 3.11.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 2af274323f..f58f8f1539 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4098984815..2b5106ff83 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.10.1 + 3.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 81ff7aa1e1..3782216e48 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.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Let sphinx add parameter description to documentation (`#651 `_) * Contributors: Christoph Fröhlich, gwalck diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e82ef7e202..b8893108e0 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.10.1 + 3.11.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f766725611..b86820e783 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.11.0 (2023-06-24) +------------------- * Added -Wconversion flag and fix warnings (`#667 `_) * Contributors: gwalck diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5d71730f8f..eba1e97ad2 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.10.1 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 9ce288b9cd9caabb1d29df6fb4e1baefa52781cb Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 26 Jun 2023 10:38:05 -0700 Subject: [PATCH 302/524] [JTC] Command final waypoint identically when traj_point_active_ptr_ is nullptr (#682) --- .../src/joint_trajectory_controller.cpp | 102 +++++++++--------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4d994e26bf..5e3e0fd770 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -241,57 +241,6 @@ controller_interface::return_type JointTrajectoryController::update( } } - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) - { - if (use_closed_loop_pid_adapter_) - { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); - } - if (has_velocity_command_interface_) - { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); - } - if (has_effort_command_interface_) - { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } - } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; - } - const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { @@ -359,6 +308,57 @@ controller_interface::return_type JointTrajectoryController::update( set_hold_position(); RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); } + + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) + { + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_error_.positions[i], state_error_.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } } } From 76b1ce7da5e7a102ef3a1c8c4f1b0de90ed0d137 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 1 Jul 2023 18:56:28 +0200 Subject: [PATCH 303/524] Add CI workflow to check docs (#691) --- .github/workflows/ci-check-docs.yml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 .github/workflows/ci-check-docs.yml diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/ci-check-docs.yml new file mode 100644 index 0000000000..629279615b --- /dev/null +++ b/.github/workflows/ci-check-docs.yml @@ -0,0 +1,12 @@ +name: Check Docs + +on: + workflow_dispatch: + pull_request: + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} From 9908a9cbe8e00548b457f0417eb9e643ffdc8656 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 304/524] [JTC] Extend tests (#612) --- .../test/test_trajectory_actions.cpp | 153 +- .../test/test_trajectory_controller.cpp | 1743 ++++++++--------- .../test/test_trajectory_controller_utils.hpp | 10 +- 3 files changed, 958 insertions(+), 948 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ef0a11d961..fc2dd42314 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -197,7 +197,24 @@ class TestTrajectoryActions : public TrajectoryControllerTest } }; -TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class TestTrajectoryActionsTestParameterized +: public TestTrajectoryActions, + public ::testing::WithParamInterface< + std::tuple, std::vector>> +{ +public: + virtual void SetUp() + { + TestTrajectoryActions::SetUp(); + command_interface_types_ = std::get<0>(GetParam()); + state_interface_types_ = std::get<1>(GetParam()); + } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { SetUpExecutor(); SetUpControllerHardware(); @@ -223,12 +240,15 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_EQ(1.0, joint_pos_[0]); + EXPECT_EQ(2.0, joint_pos_[1]); + EXPECT_EQ(3.0, joint_pos_[2]); + } } -TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { SetUpExecutor(); SetUpControllerHardware(); @@ -270,11 +290,18 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + } } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented +*/ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { // set tolerance parameters @@ -308,11 +335,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } } +/** + * Makes sense with position command interface only, + * because no integration to position state interface is implemented +*/ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { // set tolerance parameters @@ -363,12 +397,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_state_tolerances_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -415,12 +452,15 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) { // set joint tolerance parameters const double goal_tol = 0.1; @@ -465,12 +505,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) +TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) { // set joint tolerance parameters const double state_tol = 0.0001; @@ -512,12 +555,15 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + } } -TEST_F(TestTrajectoryActions, test_cancel_hold_position) +TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) { SetUpExecutor(); SetUpControllerHardware(); @@ -561,7 +607,54 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) // run an update, it should be holding updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_EQ(prev_pos1, joint_pos_[0]); + EXPECT_EQ(prev_pos2, joint_pos_[1]); + EXPECT_EQ(prev_pos3, joint_pos_[2]); + } } + +// position controllers +INSTANTIATE_TEST_SUITE_P( + PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple(std::vector({"position"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position"}), + std::vector({"position", "velocity", "acceleration"})))); + +// position_velocity controllers +INSTANTIATE_TEST_SUITE_P( + PositionVelocityTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "velocity"}), std::vector({"position"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllersAction, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TestTrajectoryActionsTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index b1188eede7..714d3c8d49 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -199,119 +199,6 @@ TEST_P(TrajectoryControllerTestParameterized, activate) executor.cancel(); } -// TEST_F(TestTrajectoryController, activation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points {{{3.3, 4.4, 5.5}}}; -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// executor.cancel(); -// } - -// TEST_F(TestTrajectoryController, reactivation) { -// auto traj_controller = std::make_shared( -// joint_names_, op_mode_); -// auto ret = traj_controller->init(test_robot_, controller_name_); -// if (ret != controller_interface::return_type::OK) { -// FAIL(); -// } -// -// auto traj_node = traj_controller->get_node(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// -// auto state = traj_controller_->configure(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// -// // wait for the subscriber and publisher to completely setup -// std::this_thread::sleep_for(std::chrono::seconds(2)); -// -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// // *INDENT-OFF* -// std::vector> points { -// {{3.3, 4.4, 5.5}}, -// {{7.7, 8.8, 9.9}}, -// {{10.10, 11.11, 12.12}} -// }; -// // *INDENT-ON* -// publish(time_from_start, points, rclcpp::Time()); -// // wait for msg is be published to the system -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// executor.spin_once(); -// -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // deactivated -// // wait so controller process the second point when deactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// state = traj_controller_->deactivate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // no change in hw position -// EXPECT_EQ(3.3, joint_pos_[0]); -// EXPECT_EQ(4.4, joint_pos_[1]); -// EXPECT_EQ(5.5, joint_pos_[2]); -// -// // reactivated -// // wait so controller process the third point when reactivated -// std::this_thread::sleep_for(std::chrono::milliseconds(3000)); -// state = traj_node->activate(); -// ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); -// resource_manager_->read(); -// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// resource_manager_->write(); -// -// // change in hw position to 3rd point -// EXPECT_EQ(10.10, joint_pos_[0]); -// EXPECT_EQ(11.11, joint_pos_[1]); -// EXPECT_EQ(12.12, joint_pos_[2]); -// -// executor.cancel(); -// } - TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; @@ -338,7 +225,10 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + // update for 0.25 seconds + // TODO(anyone): should the controller even allow calling update() when it is not active? + // update loop receives a new msg and updates accordingly updateController(rclcpp::Duration::from_seconds(0.25)); // should be home pose again @@ -428,25 +318,28 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_LE(0.0, joint_eff_[2]); } - // cleanup - state = traj_controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + // reactivated + // wait so controller process the third point when reactivated + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + // TODO(anyone) test copied from ROS 1: it fails now! + // should the old trajectory really be processed after reactivation? +#if 0 + ActivateTrajectoryController(); + state = traj_controller_->get_state(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // TODO(anyone): should the controller even allow calling update() when it is not active? - // update loop receives a new msg and updates accordingly - traj_controller_->update( - rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); + updateController(rclcpp::Duration::from_seconds(0.5)); + // traj_controller_->update( + // rclcpp::Time(static_cast(0.75 * 1e9)), rclcpp::Duration::from_seconds(0.01)); + // change in hw position to 3rd point if (traj_controller_->has_position_command_interface()) { - // otherwise this won't be updated - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + EXPECT_NEAR(10.10, joint_pos_[0], allowed_delta); + EXPECT_NEAR(11.11, joint_pos_[1], allowed_delta); + EXPECT_NEAR(12.12, joint_pos_[2], allowed_delta); } - - // state = traj_controller_->get_node()->configure(); - // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); +#endif executor.cancel(); } @@ -723,14 +616,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) if (traj_controller_->has_velocity_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - EXPECT_GT(0.0, joint_vel_[2]); // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], @@ -739,6 +627,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); // is error of positions[2] normalized? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * allowed_delta); @@ -746,819 +635,839 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) else { // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); } -// /** -// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from -// * internal controller order -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// { -// trajectory_msgs::msg::JointTrajectory traj_msg; -// const std::vector jumbled_joint_names{ -// joint_names_[1], joint_names_[2], joint_names_[0]}; -// traj_msg.joint_names = jumbled_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(3); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 3.0; -// traj_msg.points[0].positions[2] = 1.0; - -// if (traj_controller_->has_velocity_command_interface()) -// { -// traj_msg.points[0].velocities.resize(3); -// traj_msg.points[0].velocities[0] = -0.1; -// traj_msg.points[0].velocities[1] = -0.1; -// traj_msg.points[0].velocities[2] = -0.1; -// } - -// if (traj_controller_->has_effort_command_interface()) -// { -// traj_msg.points[0].effort.resize(3); -// traj_msg.points[0].effort[0] = -0.1; -// traj_msg.points[0].effort[1] = -0.1; -// traj_msg.points[0].effort[2] = -0.1; -// } - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.25 seconds -// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? -// // Currently COMMON_THRESHOLD is adjusted. -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); -// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); -// } +/** + * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from + * internal controller order + */ +TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor); + { + trajectory_msgs::msg::JointTrajectory traj_msg; + const std::vector jumbled_joint_names{ + joint_names_[1], joint_names_[2], joint_names_[0]}; + traj_msg.joint_names = jumbled_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(3); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 3.0; + traj_msg.points[0].positions[2] = 1.0; + + if (traj_controller_->has_velocity_command_interface()) + { + traj_msg.points[0].velocities.resize(3); + traj_msg.points[0].velocities[0] = -0.1; + traj_msg.points[0].velocities[1] = -0.1; + traj_msg.points[0].velocities[2] = -0.1; + } -// if (traj_controller_->has_velocity_command_interface()) -// { -// EXPECT_GT(0.0, joint_vel_[0]); -// EXPECT_GT(0.0, joint_vel_[1]); -// EXPECT_GT(0.0, joint_vel_[2]); -// } + if (traj_controller_->has_effort_command_interface()) + { + traj_msg.points[0].effort.resize(3); + traj_msg.points[0].effort[0] = -0.1; + traj_msg.points[0].effort[1] = -0.1; + traj_msg.points[0].effort[2] = -0.1; + } -// if (traj_controller_->has_effort_command_interface()) -// { -// EXPECT_GT(0.0, joint_eff_[0]); -// EXPECT_GT(0.0, joint_eff_[1]); -// EXPECT_GT(0.0, joint_eff_[2]); -// } -// } + trajectory_publisher_->publish(traj_msg); + } -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + traj_controller_->wait_for_trajectory(executor); + // update for 0.25 seconds + // TODO(destogl): Make this time a bit shorter to increase stability on the CI? + // Currently COMMON_THRESHOLD is adjusted. + updateController(rclcpp::Duration::from_seconds(0.25)); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// trajectory_msgs::msg::JointTrajectory traj_msg; + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = -// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); -// traj_msg.points[0].velocities[1] = -// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; - -// if (traj_controller_->has_position_command_interface()) -// { -// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); -// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "Joint 3 command should be current position"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); -// EXPECT_NEAR(0.0, joint_vel_[2], threshold) -// << "Joint 3 velocity should be 0.0 since it's not in the goal"; -// } - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != -// command_interface_types_.end()) -// { -// // estimate the sign of the velocity -// // joint rotates forward -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); -// EXPECT_TRUE( -// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); -// EXPECT_NEAR(0.0, joint_eff_[2], threshold) -// << "Joint 3 effort should be 0.0 since it's not in the goal"; -// } -// // TODO(anyone): add here ckecks for acceleration commands - -// executor.cancel(); -// } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } + // TODO(anyone): add here checks for acceleration commands +} -// /** -// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled -// * joints without allow_partial_joints_goal -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// const double initial_joint1_cmd = joint_pos_[0]; -// const double initial_joint2_cmd = joint_pos_[1]; -// const double initial_joint3_cmd = joint_pos_[2]; -// const double initial_joint_vel = 0.0; -// const double initial_joint_acc = 0.0; -// trajectory_msgs::msg::JointTrajectory traj_msg; + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + trajectory_msgs::msg::JointTrajectory traj_msg; -// { -// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; -// traj_msg.joint_names = partial_joint_names; -// traj_msg.header.stamp = rclcpp::Time(0); -// traj_msg.points.resize(1); - -// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// traj_msg.points[0].positions.resize(2); -// traj_msg.points[0].positions[0] = 2.0; -// traj_msg.points[0].positions[1] = 1.0; -// traj_msg.points[0].velocities.resize(2); -// traj_msg.points[0].velocities[0] = 2.0; -// traj_msg.points[0].velocities[1] = 1.0; - -// trajectory_publisher_->publish(traj_msg); -// } - -// traj_controller_->wait_for_trajectory(executor); -// // update for 0.5 seconds -// updateController(rclcpp::Duration::from_seconds(0.25)); - -// double threshold = 0.001; -// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) -// << "All joints command should be current position because goal was rejected"; -// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) -// << "All joints command should be current position because goal was rejected"; - -// if ( -// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) -// << "All joints velocities should be 0.0 because goal was rejected"; -// } - -// if ( -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != -// command_interface_types_.end()) -// { -// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) -// << "All joints accelerations should be 0.0 because goal was rejected"; -// } - -// executor.cancel(); -// } + { + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = + copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + traj_msg.points[0].velocities[1] = + copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + + trajectory_publisher_->publish(traj_msg); + } -// /** -// * @brief invalid_message Test mismatched joint and reference vector sizes -// */ -// TEST_P(TrajectoryControllerTestParameterized, invalid_message) -// { -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); -// rclcpp::Parameter allow_integration_parameters( -// "allow_integration_in_goal_trajectories", false); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController( -// true, {partial_joints_parameters, allow_integration_parameters}, &executor); - -// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - -// good_traj_msg.joint_names = joint_names_; -// good_traj_msg.header.stamp = rclcpp::Time(0); -// good_traj_msg.points.resize(1); -// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// good_traj_msg.points[0].positions.resize(1); -// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; -// good_traj_msg.points[0].velocities.resize(1); -// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // Incompatible joint names -// traj_msg = good_traj_msg; -// traj_msg.joint_names = {"bad_name"}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few efforts -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].effort = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Non-strictly increasing waypoint times -// traj_msg = good_traj_msg; -// traj_msg.points.push_back(traj_msg.points.front()); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(0.25)); -// /// With allow_integration_in_goal_trajectories parameter trajectory missing position or -// /// velocities are accepted -// TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) -// { -// rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); - -// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - -// good_traj_msg.joint_names = joint_names_; -// good_traj_msg.header.stamp = rclcpp::Time(0); -// good_traj_msg.points.resize(1); -// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); -// good_traj_msg.points[0].positions.resize(1); -// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; -// good_traj_msg.points[0].velocities.resize(1); -// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; -// good_traj_msg.points[0].accelerations.resize(1); -// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - -// // No position data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // No position and velocity data -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // All empty -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions.clear(); -// traj_msg.points[0].velocities.clear(); -// traj_msg.points[0].accelerations.clear(); -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too many positions -// traj_msg = good_traj_msg; -// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few velocities -// traj_msg = good_traj_msg; -// traj_msg.points[0].velocities = {1.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - -// // Incompatible data sizes, too few accelerations -// traj_msg = good_traj_msg; -// traj_msg.points[0].accelerations = {2.0}; -// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -// } + double threshold = 0.001; -// /** -// * @brief test_trajectory_replace Test replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}}}; -// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; -// std::vector> points_partial_new{{1.5}}; -// std::vector> points_partial_new_velocities{{0.15}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_actual.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// expected_desired.velocities = { -// points_old_velocities[0].begin(), points_old_velocities[0].end()}; -// // Check that we reached end of points_old trajectory -// // Denis: delta was 0.1 with 0.2 works for me -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); -// points_partial_new_velocities[0][0] = -// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); -// publish( -// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); - -// // Replaced trajectory is a mix of previous and current goal -// expected_desired.positions[0] = points_partial_new[0][0]; -// expected_desired.positions[1] = points_old[0][1]; -// expected_desired.positions[2] = points_old[0][2]; -// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; -// expected_desired.velocities[1] = 0.0; -// expected_desired.velocities[2] = 0.0; -// expected_actual = expected_desired; -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "Joint 3 command should be current position"; + } -// /** -// * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// // TODO(anyone): add expectations for velocities and accelerations -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}}}; - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0] trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); -// // New trajectory will end before current time -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; -// expected_desired = expected_actual; -// std::cout << "Sending old trajectory" << std::endl; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_velocity_command_interface()) + { + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_NEAR(0.0, joint_vel_[2], threshold) + << "Joint 3 velocity should be 0.0 since it's not in the goal"; + } -// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); - -// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; -// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; -// publish(time_from_start, points_old, rclcpp::Time()); -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// RCLCPP_INFO( -// traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); -// // New trajectory first point is in the past, second is in the future -// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); -// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; -// expected_desired = expected_actual; -// publish(time_from_start, points_new, new_traj_start); -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -// } + if (traj_controller_->has_effort_command_interface()) + { + // estimate the sign of the effort + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], threshold) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } + // TODO(anyone): add here checks for acceleration commands -// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) -// { -// SetUpTrajectoryController(); -// auto traj_node = traj_controller_->get_node(); -// RCLCPP_WARN( -// traj_node->get_logger(), -// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); -// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ -// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 -// return; - -// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(traj_node->get_node_base_interface()); -// subscribeToState(); -// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); -// traj_node->set_parameter(partial_joints_parameters); -// traj_controller_->get_node()->configure(); -// traj_controller_->get_node()->activate(); - -// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; -// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; -// std::vector> partial_traj{ -// {{-1., -2.}, -// { -// -2., -// -4, -// }}}; -// std::vector> partial_traj_velocities{ -// {{-0.1, -0.2}, -// { -// -0.2, -// -0.4, -// }}}; -// const auto delay = std::chrono::milliseconds(500); -// builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; -// // Send full trajectory -// publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); -// // Sleep until first waypoint of full trajectory - -// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; -// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; -// expected_desired = expected_actual; -// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] -// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - -// // Send partial trajectory starting after full trajecotry is complete -// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); -// publish( -// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); -// // Wait until the end start and end of partial traj - -// expected_actual.positions = { -// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; -// expected_desired = expected_actual; - -// waitAndCompareState( -// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); -// } + executor.cancel(); +} -// TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in opposite direction from command -// // (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_LT(joint_pos_[0], first_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there should be a "jump" in the goal direction from command -// // (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // Expect backward commands at first -// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } +/** + * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled + * joints without allow_partial_joints_goal + */ +TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); -// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// // default if false so it will not be actually set parameter -// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - -// // goal setup -// std::vector first_goal = {3.3, 4.4, 5.5}; -// std::vector second_goal = {6.6, 8.8, 11.0}; -// double state_from_command_offset = 0.3; - -// // send msg -// builtin_interfaces::msg::Duration time_from_start; -// time_from_start.sec = 1; -// time_from_start.nanosec = 0; -// std::vector> points{{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); -// updateController(rclcpp::Duration::from_seconds(1.1)); - -// if (traj_controller_->has_position_command_interface()) -// { -// // JTC is executing trajectory in open-loop therefore: -// // - internal state does not have to be updated (in this test-case it shouldn't) -// // - internal command is updated -// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - -// // Move joint further in the same direction as before (to the second goal) -// points = {{second_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in opposite direction from -// // command (towards the state value) -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be backward commands -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the second goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - -// // State interface should have offset from the command before starting a new trajectory -// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - -// // Move joint back to the first goal -// points = {{first_goal}}; -// publish(time_from_start, points, rclcpp::Time()); -// traj_controller_->wait_for_trajectory(executor); - -// // One the first update(s) there **should not** be a "jump" in the goal direction from -// // command (towards the state value) -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// // There should not be a jump toward commands -// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// EXPECT_GT(joint_pos_[0], first_goal[0]); -// EXPECT_LT(joint_pos_[0], second_goal[0]); - -// // Finally the first goal will be commanded/reached -// updateController(rclcpp::Duration::from_seconds(1.1)); -// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); -// } - -// executor.cancel(); -// } + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// // 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 -// 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); + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; + const double initial_joint3_cmd = joint_pos_[2]; + trajectory_msgs::msg::JointTrajectory traj_msg; -// // 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(true, {is_open_loop_parameters}, &executor, true); + { + std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + traj_msg.joint_names = partial_joint_names; + traj_msg.header.stamp = rclcpp::Time(0); + traj_msg.points.resize(1); + + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(2); + traj_msg.points[0].positions[0] = 2.0; + traj_msg.points[0].positions[1] = 1.0; + traj_msg.points[0].velocities.resize(2); + traj_msg.points[0].velocities[0] = 2.0; + traj_msg.points[0].velocities[1] = 1.0; + + trajectory_publisher_->publish(traj_msg); + } -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + traj_controller_->wait_for_trajectory(executor); + // update for 0.5 seconds + updateController(rclcpp::Duration::from_seconds(0.25)); -// for (size_t i = 0; i < 3; ++i) -// { -// 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() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); -// } -// } - -// executor.cancel(); -// } + double threshold = 0.001; -// // Testing that values are read from state 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); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + << "All joints command should be current position because goal was rejected"; + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "All joints command should be current position because goal was rejected"; + } -// // set command values to NaN -// for (size_t i = 0; i < 3; ++i) -// { -// joint_pos_[i] = 3.1 + i; -// joint_vel_[i] = 0.25 + i; -// joint_acc_[i] = 0.02 + i / 10.0; -// } -// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + << "All joints velocities should be 0.0 because goal was rejected"; + } -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + << "All joints accelerations should be 0.0 because goal was rejected"; + } -// 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() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } - -// // check acceleration -// if ( -// std::find( -// state_interface_types_.begin(), state_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && -// std::find( -// command_interface_types_.begin(), command_interface_types_.end(), -// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) -// { -// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); -// } -// } - -// executor.cancel(); -// } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + << "All joints efforts should be 0.0 because goal was rejected"; + } + + executor.cancel(); +} + +/** + * @brief invalid_message Test mismatched joint and reference vector sizes + */ +TEST_P(TrajectoryControllerTestParameterized, invalid_message) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController( + executor, true, {partial_joints_parameters, allow_integration_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // Incompatible joint names + traj_msg = good_traj_msg; + traj_msg.joint_names = {"bad_name"}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few efforts + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].effort = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Non-strictly increasing waypoint times + traj_msg = good_traj_msg; + traj_msg.points.push_back(traj_msg.points.front()); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/// With allow_integration_in_goal_trajectories parameter trajectory missing position or +/// velocities are accepted +TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +{ + rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {allow_integration_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + + good_traj_msg.joint_names = joint_names_; + good_traj_msg.header.stamp = rclcpp::Time(0); + good_traj_msg.points.resize(1); + good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + good_traj_msg.points[0].positions.resize(1); + good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + good_traj_msg.points[0].velocities.resize(1); + good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + good_traj_msg.points[0].accelerations.resize(1); + good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + + // No position data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // No position and velocity data + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // All empty + traj_msg = good_traj_msg; + traj_msg.points[0].positions.clear(); + traj_msg.points[0].velocities.clear(); + traj_msg.points[0].accelerations.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too many positions + traj_msg = good_traj_msg; + traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few velocities + traj_msg = good_traj_msg; + traj_msg.points[0].velocities = {1.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Incompatible data sizes, too few accelerations + traj_msg = good_traj_msg; + traj_msg.points[0].accelerations = {2.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief test_trajectory_replace Test replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + + subscribeToState(); + + std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; + std::vector> points_partial_new{{1.5}}; + std::vector> points_partial_new_velocities{{0.15}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; + expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + // Check that we reached end of points_old trajectory + // Denis: delta was 0.1 with 0.2 works for me + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); + points_partial_new_velocities[0][0] = + copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + + // Replaced trajectory is a mix of previous and current goal + expected_desired.positions[0] = points_partial_new[0][0]; + expected_desired.positions[1] = points_old[0][1]; + expected_desired.positions[2] = points_old[0][2]; + expected_desired.velocities[0] = points_partial_new_velocities[0][0]; + expected_desired.velocities[1] = 0.0; + expected_desired.velocities[2] = 0.0; + expected_actual = expected_desired; + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +/** + * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory + */ +TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}); + subscribeToState(); + + // TODO(anyone): add expectations for velocities and accelerations + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}}}; + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0] trajectory + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); + // New trajectory will end before current time + rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired = expected_actual; + std::cout << "Sending old trajectory" << std::endl; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}); + subscribeToState(); + + std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; + std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + publish(time_from_start, points_old, rclcpp::Time()); + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + RCLCPP_INFO( + traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); + // New trajectory first point is in the past, second is in the future + rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; + expected_desired = expected_actual; + publish(time_from_start, points_new, new_traj_start); + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +} + +TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +{ + rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + subscribeToState(); + + RCLCPP_WARN( + traj_controller_->get_node()->get_logger(), + "Test disabled until current_trajectory is taken into account when adding a new trajectory."); + // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ + // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 + return; + + // *INDENT-OFF* + std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; + std::vector> partial_traj{{{-1., -2.}, {-2., -4}}}; + std::vector> partial_traj_velocities{{{-0.1, -0.2}, {-0.2, -0.4}}}; + // *INDENT-ON* + const auto delay = std::chrono::milliseconds(500); + builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; + // Send full trajectory + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); + // Sleep until first waypoint of full trajectory + + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; + expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; + expected_desired = expected_actual; + // Check that we reached end of points_old[0]trajectory and are starting points_old[1] + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + + // Send partial trajectory starting after full trajecotry is complete + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); + publish( + points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); + // Wait until the end start and end of partial traj + + expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; + expected_desired = expected_actual; + + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); +} + +// TODO(destogl) this test fails with errors +// second publish() gives an error, because end time is before current time +// as well as +// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, +// which exceeds COMMON_THRESHOLD, where +// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, +// 2: joint_pos_[0] evaluates to 6.2700020099999998, and +// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. +// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. +// test_jump_when_state_tracking_error_updated/0, where GetParam() = +// ({ "position" }, { "position" }) (3372 ms) + +#if 0 +TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // default if false so it will not be actually set parameter + rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; + std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points{{first_goal}}; + publish(time_from_start, points, + rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, + rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } + + executor.cancel(); +} +#endif + +// TODO(destogl) this test fails +// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, +// which exceeds COMMON_THRESHOLD, where +// 2: second_goal[0] evaluates to 6.5999999999999996, +// 2: joint_pos_[0] evaluates to 6.5670133649999993, and +// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. +// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. +// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = +// ({ "position" }, { "position", "velocity" }) (3374 ms) +#if 0 +TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + // goal setup + std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector second_goal = {6.6, 8.8, 11.0}; + double state_from_command_offset = 0.3; + + // send msg + builtin_interfaces::msg::Duration time_from_start; + time_from_start.sec = 1; + time_from_start.nanosec = 0; + std::vector> points{{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(1.1)); + + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + updateController(rclcpp::Duration::from_seconds(0.01)); + // There should not be a jump toward commands + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + updateController(rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } + + executor.cancel(); +} +#endif + +// Testing that values are read from state interfaces when hardware is started for the first +// time and hardware state has offset --> this is indicated by NaN values in state 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, true, {is_open_loop_parameters}, true); + + 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_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()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[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()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + } + } + + executor.cancel(); +} + +// Testing that values are read from state 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 + for (size_t i = 0; i < 3; ++i) + { + joint_pos_[i] = 3.1 + i; + joint_vel_[i] = 0.25 + i; + joint_acc_[i] = 0.02 + i / 10.0; + } + SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + + 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()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[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()) + { + EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + } + } + + executor.cancel(); +} // position controllers INSTANTIATE_TEST_SUITE_P( @@ -1608,15 +1517,15 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"velocity"}), std::vector({"position", "velocity", "acceleration"})))); -// // only effort controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"effort"}), std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"effort"}), -// std::vector({"position", "velocity", "acceleration"})))); +// only effort controller +INSTANTIATE_TEST_SUITE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); // TODO(destogl): this tests should be changed because we are using `generate_parameters_library` // TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8f5f64d57f..9855341edf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -110,6 +110,8 @@ class TestableJointTrajectoryController bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_acceleration_command_interface() { return has_acceleration_command_interface_; } + bool has_effort_command_interface() { return has_effort_command_interface_; } bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } @@ -364,7 +366,13 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + // TODO(christophfroehlich): use the node's clock here for internal comparison + // if using RCL_STEADY_TIME -> + // C++ exception with description + // "can't compare times with different time sources" thrown in the test body. + // traj_controller_->update(clock.now(), clock.now() - start_time); + // maybe we can set the node clock to use RCL_STEADY_TIME too? + traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time); } } From 8674f2db00fd7dbd53971c0f97bac906b9b0361a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 305/524] Increase action tests timeout (#680) --- joint_trajectory_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 35223016b7..0552e0bc86 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -83,6 +83,7 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory_actions test/test_trajectory_actions.cpp ) + set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) From 7e13d1d4e48990f3e3a7aef78eb38d377ae8f161 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 306/524] [JTC] Fix time sources and wrong checks in tests (#686) * Fix time sources and wrong checks in tests * Use time from update-method instead of node clock * Readd test of last command in test_goal_tolerances_fail --------- Co-authored-by: Bence Magyar --- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_actions.cpp | 25 ++++++++++--------- .../test/test_trajectory_controller.cpp | 20 +++++++++------ .../test/test_trajectory_controller_utils.hpp | 8 +----- 4 files changed, 28 insertions(+), 27 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e3e0fd770..3b8f5f89bd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -231,7 +231,7 @@ controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - time_difference = get_node()->now().seconds() - traj_end.seconds(); + time_difference = time.seconds() - traj_end.seconds(); if (time_difference > default_tolerances_.goal_time_tolerance) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fc2dd42314..085c20cacd 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -66,11 +66,14 @@ class TestTrajectoryActions : public TrajectoryControllerTest goal_options_.feedback_callback = nullptr; } - void SetUpExecutor(const std::vector & parameters = {}) + void SetUpExecutor( + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, true, parameters); + SetUpAndActivateTrajectoryController( + executor_, true, parameters, separate_cmd_and_state_values); SetUpActionClient(); @@ -472,15 +475,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.joint3.goal", goal_tol), rclcpp::Parameter("constraints.goal_time", goal_time)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; - // send goal + // send goal; one point only -> command is directly set to reach this goal (no interpolation) { std::vector points; JointTrajectoryPoint point; @@ -502,14 +503,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update, it should be holding the last received goal updateController(rclcpp::Duration::from_seconds(0.01)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(4.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(5.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(6.0, joint_pos_[2], COMMON_THRESHOLD); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 714d3c8d49..0c8b3ae939 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1092,7 +1092,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; std::cout << "Sending old trajectory" << std::endl; @@ -1108,23 +1109,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - + trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; + + // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; - expected_desired = expected_actual; + rclcpp::Time new_traj_start = + rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); + // it should not have accepted the new goal but finish the old one + expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; + expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -1163,7 +1168,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); publish( - points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); + points_delay, partial_traj, rclcpp::Clock(RCL_STEADY_TIME).now() + delay * 2, {}, + partial_traj_velocities); // Wait until the end start and end of partial traj expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9855341edf..97156f1a2e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -366,13 +366,7 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (clock.now() < end_time) { - // TODO(christophfroehlich): use the node's clock here for internal comparison - // if using RCL_STEADY_TIME -> - // C++ exception with description - // "can't compare times with different time sources" thrown in the test body. - // traj_controller_->update(clock.now(), clock.now() - start_time); - // maybe we can set the node clock to use RCL_STEADY_TIME too? - traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } } From f70c98649dfcddc620286b72abe868b1204c1391 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 307/524] Match feature branches (#689) --- .github/workflows/rolling-binary-build-main.yml | 4 ++++ .github/workflows/rolling-binary-build-testing.yml | 4 ++++ .github/workflows/rolling-semi-binary-build-main.yml | 4 ++++ .github/workflows/rolling-semi-binary-build-testing.yml | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index b51bbabe29..793db5d7e5 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -6,9 +6,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml index a1db89b8d9..9b480d99c3 100644 --- a/.github/workflows/rolling-binary-build-testing.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -6,9 +6,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml index eca9483438..8b395e5163 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -5,9 +5,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 62214adae9..630881dc0a 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -5,9 +5,13 @@ on: workflow_dispatch: branches: - master + - '*feature*' + - '*feature/**' pull_request: branches: - master + - '*feature*' + - '*feature/**' push: branches: - master From ddb02e8e2422596b9870062eee30653387b58b5c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 308/524] Revert "[JTC] Command final waypoint identically when traj_point_active_ptr_ is nullptr (#682)" This reverts commit 9ce288b9cd9caabb1d29df6fb4e1baefa52781cb. --- .../src/joint_trajectory_controller.cpp | 102 +++++++++--------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3b8f5f89bd..c870791daf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -241,6 +241,57 @@ controller_interface::return_type JointTrajectoryController::update( } } + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) + { + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_error_.positions[i], state_error_.velocities[i], + (uint64_t)period.nanoseconds()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter_) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } + const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { @@ -308,57 +359,6 @@ controller_interface::return_type JointTrajectoryController::update( set_hold_position(); RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); } - - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) - { - if (use_closed_loop_pid_adapter_) - { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); - } - if (has_velocity_command_interface_) - { - if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); - } - if (has_effort_command_interface_) - { - if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } - } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; - } } } From ff6e81d45dee018cd852e94978bf1c5627b0c3fd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 309/524] Add test for velocity error with effort cmd interface (#690) --- .../test/test_trajectory_controller.cpp | 90 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 25 ++++-- 2 files changed, 107 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0c8b3ae939..bd547dc6be 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -673,6 +673,96 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) executor.cancel(); } +/** + * @brief check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + if ( + (traj_controller_->has_velocity_command_interface() && + !traj_controller_->has_position_command_interface() && + !traj_controller_->has_effort_command_interface() && + !traj_controller_->has_acceleration_command_interface() && + !traj_controller_->is_open_loop()) || + traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); + } +} + +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points_positions{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}}; + // *INDENT-ON* + publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); + EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); + EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + } + if (traj_controller_->has_acceleration_state_interface()) + { + EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); + EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + } + + // no change in state interface should happen + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + } + // is the velocity error correct? + if ( + traj_controller_->use_closed_loop_pid_adapter() // always needed for PID controller + || (traj_controller_->has_velocity_state_interface() && + traj_controller_->has_velocity_command_interface())) + { + // don't check against a value, because spline intepolation might overshoot depending on + // interface combinations + EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + } + + executor.cancel(); +} + /** * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from * internal controller order diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 97156f1a2e..4dd5f1a780 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -105,6 +105,11 @@ class TestableJointTrajectoryController { return last_commanded_state_; } + bool has_position_state_interface() { return has_position_state_interface_; } + + bool has_velocity_state_interface() { return has_velocity_state_interface_; } + + bool has_acceleration_state_interface() { return has_acceleration_state_interface_; } bool has_position_command_interface() { return has_position_command_interface_; } @@ -116,6 +121,8 @@ class TestableJointTrajectoryController bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + bool is_open_loop() { return params_.open_loop_control; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -294,13 +301,14 @@ class TrajectoryControllerTest : public ::testing::Test /// Publish trajectory msgs with multiple points /** * delay_btwn_points - delay between each points - * points - vector of trajectories. One point per controlled joint + * points_positions - vector of trajectory-positions. One point per controlled joint * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided * points + * points - vector of trajectory-velocities. One point per controlled joint */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, rclcpp::Time start_time, + const std::vector> & points_positions, rclcpp::Time start_time, const std::vector & joint_names = {}, const std::vector> & points_velocities = {}) { @@ -320,14 +328,15 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_msgs::msg::JointTrajectory traj_msg; if (joint_names.empty()) { - traj_msg.joint_names = {joint_names_.begin(), joint_names_.begin() + points[0].size()}; + traj_msg.joint_names = { + joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; } else { traj_msg.joint_names = joint_names; } traj_msg.header.stamp = start_time; - traj_msg.points.resize(points.size()); + traj_msg.points.resize(points_positions.size()); builtin_interfaces::msg::Duration duration_msg; duration_msg.sec = delay_btwn_points.sec; @@ -335,14 +344,14 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Duration duration(duration_msg); rclcpp::Duration duration_total(duration_msg); - for (size_t index = 0; index < points.size(); ++index) + for (size_t index = 0; index < points_positions.size(); ++index) { traj_msg.points[index].time_from_start = duration_total; - traj_msg.points[index].positions.resize(points[index].size()); - for (size_t j = 0; j < points[index].size(); ++j) + traj_msg.points[index].positions.resize(points_positions[index].size()); + for (size_t j = 0; j < points_positions[index].size(); ++j) { - traj_msg.points[index].positions[j] = points[index][j]; + traj_msg.points[index].positions[j] = points_positions[index][j]; } duration_total = duration_total + duration; } From 12d46f20f666fab3f74d5bb33841a7bcc1857299 Mon Sep 17 00:00:00 2001 From: Lars Tingelstad Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 310/524] Compute velocity errors when using an effort command interface (#679) (cherry picked from commit 2e0da5d2f0a8462f735e44c5b5ad0a58fe11aedb) --- .../src/joint_trajectory_controller.cpp | 4 +++- joint_trajectory_controller/test/test_trajectory_actions.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c870791daf..153515d089 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -133,7 +133,9 @@ controller_interface::return_type JointTrajectoryController::update( { error.positions[index] = desired.positions[index] - current.positions[index]; } - if (has_velocity_state_interface_ && has_velocity_command_interface_) + if ( + has_velocity_state_interface_ && + (has_velocity_command_interface_ || has_effort_command_interface_)) { error.velocities[index] = desired.velocities[index] - current.velocities[index]; } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 085c20cacd..6482b8e921 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -304,7 +304,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal /** * Makes sense with position command interface only, * because no integration to position state interface is implemented -*/ + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { // set tolerance parameters @@ -349,7 +349,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) /** * Makes sense with position command interface only, * because no integration to position state interface is implemented -*/ + */ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { // set tolerance parameters From 99b67d73dc8223b0aca412e5d546dd26dd56d0de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 311/524] [JTC] Reject trajectories with nonzero terminal velocity (#567) * Reject receiving trajectory of last velocity point is non-zero * Update docs * Add tests * Change to parameterized test * Rename parameter * not true -> false --------- Co-authored-by: Bence Magyar --- joint_trajectory_controller/doc/userdoc.rst | 5 + .../src/joint_trajectory_controller.cpp | 23 ++++ ...oint_trajectory_controller_parameters.yaml | 5 + .../test/test_trajectory_actions.cpp | 124 ++++++++++++++++++ .../test/test_trajectory_controller.cpp | 16 ++- 5 files changed, 170 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 7529a60918..b04f1109e3 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -175,6 +175,11 @@ open_loop_control (boolean) Default: false +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 + constraints (structure) Default values for tolerances if no explicit values are states in JointTrajectory message. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 153515d089..dccb9fceff 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -64,6 +64,15 @@ 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; } @@ -1321,6 +1330,20 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + if (!params_.allow_nonzero_velocity_at_trajectory_end) + { + for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) + { + if (trajectory.points.back().velocities.at(i) != 0.) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); + return false; + } + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 90ca12a268..77eaf3537f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -66,6 +66,11 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } + allow_nonzero_velocity_at_trajectory_end: { + type: bool, + default_value: true, + description: "If false, the last velocity point has to be zero or the goal will be rejected", + } gains: __map_joints: p: { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 6482b8e921..fdea551c30 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -616,6 +616,130 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) } } +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + // will be accepted despite nonzero last point + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) +{ + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + SetUpExecutor(params); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal with nonzero last velocities + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 4.0; + point2.velocities[1] = 5.0; + point2.velocities[2] = 6.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_FALSE(gh_future.get()); + + // send goal with last velocity being zero + { + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.0); + point1.positions.resize(joint_names_.size()); + point1.velocities.resize(joint_names_.size()); + + point1.positions[0] = 4.0; + point1.positions[1] = 5.0; + point1.positions[2] = 6.0; + point1.velocities[0] = 4.0; + point1.velocities[1] = 5.0; + point1.velocities[2] = 6.0; + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.1); + point2.positions.resize(joint_names_.size()); + point2.velocities.resize(joint_names_.size()); + + point2.positions[0] = 7.0; + point2.positions[1] = 8.0; + point2.positions[2] = 9.0; + point2.velocities[0] = 0.0; + point2.velocities[1] = 0.0; + point2.velocities[2] = 0.0; + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + EXPECT_TRUE(gh_future.get()); +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index bd547dc6be..d22498a795 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -65,6 +65,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // const auto future_handle_ = std::async(std::launch::async, spin, &executor); @@ -202,7 +204,9 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -259,6 +263,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor, false); + traj_controller_->get_node()->set_parameter( + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch SetParameters(); @@ -450,7 +456,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -557,7 +565,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true); subscribeToState(); size_t n_joints = joint_names_.size(); From 88c34631e4964eafca4674b574231f569151cc74 Mon Sep 17 00:00:00 2001 From: gwalck Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 312/524] Fixed update period computation in test (#693) --- .../test/test_trajectory_controller_utils.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4dd5f1a780..5ec0f4f29e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -373,9 +373,13 @@ class TrajectoryControllerTest : public ::testing::Test auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; + auto previous_time = start_time; + while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + auto now = clock.now(); + traj_controller_->update(now, now - previous_time); + previous_time = now; } } From e49c540d4c90c9ba0bb0620c76abf8afac84dbb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 313/524] Fix namespace for parameter traits(#703) --- .../joint_trajectory_controller/validate_jtc_parameters.hpp | 5 ++--- .../src/joint_trajectory_controller_parameters.yaml | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) 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 fc6319fabf..5a656e7754 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 @@ -18,12 +18,11 @@ #include #include -#include "parameter_traits/parameter_traits.hpp" #include "rclcpp/parameter.hpp" #include "rsl/algorithm.hpp" #include "tl_expected/expected.hpp" -namespace parameter_traits +namespace joint_trajectory_controller { tl::expected command_interface_type_combinations( rclcpp::Parameter const & parameter) @@ -95,6 +94,6 @@ tl::expected state_interface_type_combinations( return {}; } -} // namespace parameter_traits +} // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 77eaf3537f..5627f1d8f5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -22,7 +22,7 @@ joint_trajectory_controller: validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], - command_interface_type_combinations: null, + "joint_trajectory_controller::command_interface_type_combinations": null, } } state_interfaces: { @@ -32,7 +32,7 @@ joint_trajectory_controller: validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], - state_interface_type_combinations: null, + "joint_trajectory_controller::state_interface_type_combinations": null, } } allow_partial_joints_goal: { From 259d3d89c10156c7e1dac9cdcd42e0b84e0f6118 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 314/524] Don't test update after cleanup --- .../test/test_trajectory_controller.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d22498a795..132a74c6fd 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -230,16 +230,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - // update for 0.25 seconds - // TODO(anyone): should the controller even allow calling update() when it is not active? - // update loop receives a new msg and updates accordingly - updateController(rclcpp::Duration::from_seconds(0.25)); - - // should be home pose again - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); - executor.cancel(); } From 126b95cc8b913175b0d65578ac9e7b2292289e53 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 17 Jul 2023 10:06:38 +0000 Subject: [PATCH 315/524] Remove reactivation test from ROS 1 --- .../test/test_trajectory_controller.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 132a74c6fd..d56a2c9b0c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -317,25 +317,12 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // reactivated // wait so controller process the third point when reactivated std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - // TODO(anyone) test copied from ROS 1: it fails now! - // should the old trajectory really be processed after reactivation? -#if 0 ActivateTrajectoryController(); state = traj_controller_->get_state(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - updateController(rclcpp::Duration::from_seconds(0.5)); - // traj_controller_->update( - // rclcpp::Time(static_cast(0.75 * 1e9)), rclcpp::Duration::from_seconds(0.01)); - - // change in hw position to 3rd point - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(10.10, joint_pos_[0], allowed_delta); - EXPECT_NEAR(11.11, joint_pos_[1], allowed_delta); - EXPECT_NEAR(12.12, joint_pos_[2], allowed_delta); - } -#endif + // TODO(christophfroehlich) add test if there is no active trajectory after + // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController) executor.cancel(); } From 8436ed549becce0b694dfe23cfa053436f2b6d42 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 17 Jul 2023 20:06:00 +0200 Subject: [PATCH 316/524] Update external ros2 controller setup link (backport #695) (#707) * Update external ros2 controller setup link (cherry picked from commit 9975ec10ef24761e2373fc77362996c804b665c4) * Update doc/writing_new_controller.rst (cherry picked from commit dd453b2f210c8b6cc34825915b32b2d34d887e91) --------- Co-authored-by: maurice Co-authored-by: Dr. Denis --- doc/writing_new_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 65ffc6795b..4d8c3752ae 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -138,4 +138,4 @@ That's it! Enjoy writing great controllers! Useful External References --------------------------- -- `Templates and scripts for generating controllers shell `_ +- `Templates and scripts for generating controllers shell `_ From 317969489c0477bf8a1e45fcd46ad55cc1d22a90 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 18 Jul 2023 07:05:09 +0100 Subject: [PATCH 317/524] Bump actions/setup-python from 4.6.1 to 4.7.0 (#710) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 8f5be014c5..8955904bea 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.6.1 + - uses: actions/setup-python@v4.7.0 with: python-version: '3.10' - name: Install system hooks From 3c7072e79c9f3c0c2ea4769abaf2dc7f47ef87ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jul 2023 18:14:01 +0200 Subject: [PATCH 318/524] Activate AdmittanceControllerTestParameterizedInvalidParameters (#711) --- .../test/test_admittance_controller.cpp | 16 ++++++++++++---- .../test/test_admittance_controller.hpp | 10 ++++++++-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index f1c3de8abd..fe1d3214e0 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -65,10 +65,18 @@ INSTANTIATE_TEST_SUITE_P( // wrong length selected axes std::make_tuple( std::string("admittance.selected_axes"), - rclcpp::ParameterValue(std::vector() = {1, 2, 3})), - // invalid robot description - std::make_tuple( - std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + rclcpp::ParameterValue(std::vector() = {1, 2, 3})) + // invalid robot description. + // TODO(anyone): deactivated, because SetUpController returns SUCCESS here? + // ,std::make_tuple( + // std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))) + )); + +// Test on_init returns ERROR when a parameter is invalid +TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters) +{ + ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR); +} TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d6c95d2e1c..8888cd700a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -456,9 +456,15 @@ class AdmittanceControllerTestParameterizedInvalidParameters static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - void SetUpController() + controller_interface::return_type SetUpController() { - AdmittanceControllerTest::SetUpController("test_admittance_controller"); + auto param_name = std::get<0>(GetParam()); + auto param_value = std::get<1>(GetParam()); + std::vector parameter_overrides; + rclcpp::Parameter param(param_name, param_value); + parameter_overrides.push_back(param); + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller", parameter_overrides); } }; From 72bdf88089d4b9158d4df1bc26968a61f7835e84 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 18 Jul 2023 19:14:52 +0100 Subject: [PATCH 319/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 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 | 16 ++++++++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 19 files changed, 72 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 6d5626ccd0..a07f3372b6 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 56feda699a..eb56a4bcd2 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Activate AdmittanceControllerTestParameterizedInvalidParameters (`#711 `_) +* Contributors: Christoph Fröhlich + 3.11.0 (2023-06-24) ------------------- * Fix cpplint (`#681 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 1b7a761aeb..0b9b4abb2b 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index fddec6b53d..c99e27900d 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.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b21b0c547d..e1cf1be3d9 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index de8adc57f1..d10ebf1b6e 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.11.0 (2023-06-24) ------------------- * Broadcaster parameters (`#650 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 796b01c03f..af62c3d352 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.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e4cb8cad5e..25dd4ec05c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5252da9c44..06f9003e1f 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.11.0 (2023-06-24) ------------------- * Broadcaster parameters (`#650 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9337cd24b2..ec928921ce 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.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 354516fbb2..6bf514fa9f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove reactivation test from ROS 1 +* Don't test update after cleanup +* Fix namespace for parameter traits(`#703 `_) +* Fixed update period computation in test (`#693 `_) +* [JTC] Reject trajectories with nonzero terminal velocity (`#567 `_) +* Compute velocity errors when using an effort command interface (`#679 `_) +* Add test for velocity error with effort cmd interface (`#690 `_) +* Revert "[JTC] Command final waypoint identically when traj_point_active_ptr\_ is nullptr (`#682 `_)" +* [JTC] Fix time sources and wrong checks in tests (`#686 `_) +* Increase action tests timeout (`#680 `_) +* [JTC] Extend tests (`#612 `_) +* [JTC] Command final waypoint identically when traj_point_active_ptr\_ is nullptr (`#682 `_) +* Contributors: Christoph Fröhlich, Ethan Gordon, Lars Tingelstad, gwalck, Bence Magyar + 3.11.0 (2023-06-24) ------------------- * jtc: fix minor typo in traj validation error msg (`#674 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 299b7b7de7..047540cd7e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4ca34fc0a5..e53ccfe66b 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f1a00b9bf4..a0ebfba35d 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Removes deprecated if-branch (`#653 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 648b093d2f..067ba3be8b 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.11.0 (2023-06-24) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 596f1fe9d3..e97c21bf78 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f58f8f1539..67322908ce 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3782216e48..5e94416668 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b86820e783..6d8c399de6 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.11.0 (2023-06-24) ------------------- * Added -Wconversion flag and fix warnings (`#667 `_) From 6a4fb70283118ed3ca4818cae1591e15c7a7ced6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 18 Jul 2023 19:15:15 +0100 Subject: [PATCH 320/524] 3.12.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a07f3372b6..f2cb336a5d 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 132cac06c0..dab9499c7d 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.11.0 + 3.12.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index eb56a4bcd2..8f5804fbe5 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- * Activate AdmittanceControllerTestParameterizedInvalidParameters (`#711 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index ba6d549877..83ca1f1b2d 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.11.0 + 3.12.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 0b9b4abb2b..c9e4604a07 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index e77fd0e0bb..d6a971ab6d 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.11.0 + 3.12.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c99e27900d..1df6139577 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index f7d2f2f361..34d236a9eb 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.11.0 + 3.12.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index e1cf1be3d9..1134b1cc09 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 567dc00741..c3280f9c9b 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.11.0 + 3.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d10ebf1b6e..7a420bb146 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index a363551958..c89df2b397 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.11.0 + 3.12.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index af62c3d352..7e33097a52 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0069f28315..de09d505ba 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.11.0 + 3.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 25dd4ec05c..797fc118ed 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 646399f14f..3e48f395af 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.11.0 + 3.12.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 06f9003e1f..9920deb233 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 993bec7ab4..4ba9bb7a55 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.11.0 + 3.12.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ec928921ce..a33b0cc011 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index da7683d69e..89a23fc15b 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.11.0 + 3.12.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 6bf514fa9f..c2e3fa332d 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.12.0 (2023-07-18) +------------------- * Remove reactivation test from ROS 1 * Don't test update after cleanup * Fix namespace for parameter traits(`#703 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index edf2ac91a9..63774d5c2e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.11.0 + 3.12.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 047540cd7e..e5c55a3569 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index d2c4eaecc6..ef214c3563 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.11.0 + 3.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index e53ccfe66b..1cc86b0d0e 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ce06ac9d71..1bfa80d078 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.11.0 + 3.12.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a0ebfba35d..73e59702f9 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 27cc252293..bc0fab74b0 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.11.0 + 3.12.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 35f7f7b942..c90806a128 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.11.0", + version="3.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 067ba3be8b..75d8985836 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c44049b9ff..1aa9b0df6d 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.11.0 + 3.12.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 5610eff43c..844e8a51d8 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.11.0", + version="3.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e97c21bf78..88400b4e3b 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5918c1491e..2b8004356a 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.11.0 + 3.12.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 67322908ce..ce14946944 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 2b5106ff83..91f6fb5a19 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.11.0 + 3.12.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 5e94416668..70b91d59b8 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.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index b8893108e0..e24d2f97c2 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.11.0 + 3.12.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6d8c399de6..c832892dd9 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.12.0 (2023-07-18) +------------------- 3.11.0 (2023-06-24) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index eba1e97ad2..d861c91806 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.11.0 + 3.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From d63b47b3cb8dc7b5220a6c318682d0ac2ceab1a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 24 Jul 2023 19:21:43 +0200 Subject: [PATCH 321/524] [Doc] Fix links (#715) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 4 ++-- doc/writing_new_controller.rst | 2 +- joint_trajectory_controller/README.md | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 95181eceb0..4a2664918a 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://control.ros.org/master/doc/getting_started/getting_started.html#building-from-source). - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) @@ -53,7 +53,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: -* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) +* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 4d8c3752ae..1543e79722 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -5,7 +5,7 @@ Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** diff --git a/joint_trajectory_controller/README.md b/joint_trajectory_controller/README.md index 9bffcc3566..874176d228 100644 --- a/joint_trajectory_controller/README.md +++ b/joint_trajectory_controller/README.md @@ -2,4 +2,4 @@ The package implements controllers to interpolate joint's trajectory. -For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/). +For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/). From af5f8c8e3cb39760178e1438f221eaf722a8556d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 25 Jul 2023 10:02:27 +0100 Subject: [PATCH 322/524] Bump ros-tooling/action-ros-ci from 0.3.2 to 0.3.3 (#718) --- .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 67d4c43375..b7698dbbe4 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.3.2 + - uses: ros-tooling/action-ros-ci@0.3.3 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 daa378297e..99a4ed66a5 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@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.2 + - uses: ros-tooling/action-ros-ci@0.3.3 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From a572381bb4364beca50136c9a5b7c5639b2d396f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 25 Jul 2023 12:52:34 +0100 Subject: [PATCH 323/524] [JTC] Reject messages with effort fields (#699) (#719) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 24 +++++++++---------- .../test/test_trajectory_controller.cpp | 21 ++-------------- 2 files changed, 14 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dccb9fceff..4b3a73c3a9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -289,14 +289,7 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_effort_command_interface_) { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } + assign_interface_from_point(joint_command_interface_[3], tmp_command_); } // store the previous command. Used in open-loop control mode @@ -1267,8 +1260,9 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + get_node()->get_logger(), + "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size, + string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; @@ -1384,11 +1378,17 @@ bool JointTrajectoryController::validate_trajectory_msg( !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + joint_count, points[i].accelerations, "accelerations", i, true)) { return false; } + // reject effort entries + if (!points[i].effort.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Trajectories with effort fields are currently not supported."); + return false; + } } return true; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d56a2c9b0c..375cdc0346 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -779,15 +779,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].velocities[1] = -0.1; traj_msg.points[0].velocities[2] = -0.1; } - - if (traj_controller_->has_effort_command_interface()) - { - traj_msg.points[0].effort.resize(3); - traj_msg.points[0].effort[0] = -0.1; - traj_msg.points[0].effort[1] = -0.1; - traj_msg.points[0].effort[2] = -0.1; - } - trajectory_publisher_->publish(traj_msg); } @@ -810,13 +801,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } - - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); - } // TODO(anyone): add here checks for acceleration commands } @@ -1027,10 +1011,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.points[0].accelerations = {1.0, 2.0}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - // Incompatible data sizes, too few efforts + // Effort is not supported in trajectory message traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].effort = {1.0, 2.0}; + traj_msg.points[0].effort = {1.0, 2.0, 3.0}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); // Non-strictly increasing waypoint times From 683696523fb193ede2d28ec107a5d18f01031344 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 27 Jul 2023 13:14:08 +0200 Subject: [PATCH 324/524] Update ci-ros-lint.yml and copyright format (#720) --- .github/workflows/ci-ros-lint.yml | 20 +++++++++++-- .../src/steering_odometry.cpp | 28 +++++++++---------- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 3149bba54d..9e08bec730 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -18,23 +18,30 @@ jobs: distribution: rolling linter: ${{ matrix.linter }} 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 - gripper_controllers position_controllers ros2_controllers ros2_controllers_test_nodes + rqt_joint_trajectory_controller + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers ament_lint_100: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: @@ -48,15 +55,22 @@ jobs: linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" 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 - gripper_controllers position_controllers ros2_controllers ros2_controllers_test_nodes + rqt_joint_trajectory_controller + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index c480dc1253..0a7dc23ef2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -1,18 +1,16 @@ -/********************************************************************* - * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - *********************************************************************/ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. /* * Author: dr. sc. Tomislav Petkovic From 76c22ffce19c7b2535bc465e21c8fa0620b0f0a6 Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Thu, 27 Jul 2023 14:28:31 -0600 Subject: [PATCH 325/524] Fix out of bound access in admittance controller (#721) --- .../admittance_rule_impl.hpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 87c16e4787..9b03924882 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -28,6 +28,9 @@ namespace admittance_controller { + +constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) + /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( const std::shared_ptr & node, const size_t num_joints) @@ -83,10 +86,10 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) { state_message_.joint_state.name = parameters_.joints; } - state_message_.mass.data.resize(6, 0.0); - state_message_.selected_axes.data.resize(6, 0); - state_message_.damping.data.resize(6, 0); - state_message_.stiffness.data.resize(6, 0); + state_message_.mass.data.resize(NUM_CARTESIAN_DOF, 0.0); + state_message_.selected_axes.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.damping.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.stiffness.data.resize(NUM_CARTESIAN_DOF, 0); state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; @@ -122,7 +125,7 @@ void AdmittanceRule::apply_parameters_update() vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); - for (size_t i = 0; i < 6; ++i) + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * @@ -331,16 +334,20 @@ void AdmittanceRule::process_wrench_measurements( const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) + { + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + state_message_.damping.data[i] = admittance_state_.damping[i]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + for (size_t i = 0; i < parameters_.joints.size(); ++i) { state_message_.joint_state.name[i] = parameters_.joints[i]; state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; From fa721709a3390b6f6465490f85a9538d216efd74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 31 Jul 2023 19:42:57 +0200 Subject: [PATCH 326/524] [JTC] Update trajectory documentation (#714) --- .../doc/new_trajectory.png | Bin 0 -> 73149 bytes .../doc/parameters.rst | 132 ++++++++++++++ .../doc/spline_acceleration.png | Bin 0 -> 34639 bytes .../doc/spline_position.png | Bin 0 -> 28511 bytes .../doc/spline_position_velocity.png | Bin 0 -> 37882 bytes .../spline_position_velocity_acceleration.png | Bin 0 -> 39812 bytes ...ion_velocity_acceleration_initialpoint.png | Bin 0 -> 39086 bytes ...ocity_acceleration_initialpoint_notime.png | Bin 0 -> 36267 bytes .../doc/spline_velocity.png | Bin 0 -> 32183 bytes .../doc/spline_wrong_points.png | Bin 0 -> 41712 bytes .../doc/trajectory.rst | 168 ++++++++++++++++++ .../doc/trajectory_replacement_future.png | Bin 0 -> 71754 bytes .../doc/trajectory_replacement_now.png | Bin 0 -> 64005 bytes .../doc/trajectory_replacement_past.png | Bin 0 -> 74230 bytes joint_trajectory_controller/doc/userdoc.rst | 167 ++--------------- 15 files changed, 316 insertions(+), 151 deletions(-) create mode 100644 joint_trajectory_controller/doc/new_trajectory.png create mode 100644 joint_trajectory_controller/doc/parameters.rst create mode 100644 joint_trajectory_controller/doc/spline_acceleration.png create mode 100644 joint_trajectory_controller/doc/spline_position.png create mode 100644 joint_trajectory_controller/doc/spline_position_velocity.png create mode 100644 joint_trajectory_controller/doc/spline_position_velocity_acceleration.png create mode 100644 joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png create mode 100644 joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png create mode 100644 joint_trajectory_controller/doc/spline_velocity.png create mode 100644 joint_trajectory_controller/doc/spline_wrong_points.png create mode 100644 joint_trajectory_controller/doc/trajectory.rst create mode 100644 joint_trajectory_controller/doc/trajectory_replacement_future.png create mode 100644 joint_trajectory_controller/doc/trajectory_replacement_now.png create mode 100644 joint_trajectory_controller/doc/trajectory_replacement_past.png diff --git a/joint_trajectory_controller/doc/new_trajectory.png b/joint_trajectory_controller/doc/new_trajectory.png new file mode 100644 index 0000000000000000000000000000000000000000..b7c07eeffee46b4602ef8f2a9333afdcab666a14 GIT binary patch literal 73149 zcmZ6z1zc3y`#n5>NGPBnAuS>Z2na~00@9&$qm(do4ILH@0@5W)NDS%FrF0|64Bg#Q zL-XIG_ulXCeb473_X^B8`|SPf=UHn#Ya61jDn~>}O$dQNh!o^yoPaG zz+f;gYkR1R*$YPtE(d3;lr1r82!s)$AoEz$BXwgE=AmgZ*8Y3@!xj7wH=aDXk%f-b zjE)In`t)HZhw&Z=VBkCVq(f6M>J=eZzlCpkzKdNO|BBShpkeGU_GOZ zlm8jcGasN92ni@nFzHJZoLZUw^xsDlQ08`~7IM$&!+DmmqOj@%xC3E@h@^{%E{CES z&Ce8!*iQ zUA~*w;S3!?HVl5b!ItEcf%>)YN*WvUYOJcXaqi|nR~~Qvv1q^`Z`(KU9D#oLMX>C@ zqms$0OLgkh|73r2{0E0P;lH1_8SSiTOLN7n2e;|QC!P|@5v&&MQsh`_D3RKKpIn%j z;nRbLdtUb-jv@#YqvdNWyb{06i5xp$WcVEX@6|Ed4=qDd4Uu?YIeOeCRkUy2yb)z~ zI3HbytE~&w=;GA>-!scU7I%#q2*rIh!t%NP7Qz`=BnA8QG|-G1$IS5BLWS}xBjfL( z|K2z_74CBnI$lGs5rBmpi%l1e;2G{z89V%~I~LnWiZakD`tLV`J$B*>dtJr*I1|6( zI@%em>Q7-kYSf3wl|ydefb)3GT>qc<3<2aUpNtfBaCd^g!LZ6prQ zQ3oT#RAidBzyI(4sReZ`P&k4k&>>)~Nji=q`oAA_2zTgMa5m93Nz4?tC2qyti26T2 z>ijb^6t9R4iL-E&!g)xj%xL3orTm}YXx6dVd}xYWa;4*{4YmV>Bb>*dr|nz*+oLRx zK2m$KJVHWW0Xkye%ezc}FYFt-uX*^5l+x3S7PZWJo|@8ED@?{UQ9$iftL4}ldafFC zmDq(yV&7y79BpFQ(_R~fD}DKTU-JFGH+b%Lq=>w@B1XJ_^#l?TMm02wSP-@k*Xzw& z*yM$7ZZF;{xbiN~$soG5pKqL4o1t&zGueQ#B@fTfeYkP>7O8uoyx z0n7w!%40q@fr#$Jc2gg5Mi=?dw8&e~NXxW*`|SQPiwGNn9K&gPgnAAATq>}@V!szt zMG4_o71U=FM`iS5`M2MBP78&{wzSw?L`wg=h4R^)$|@M4aI#=G&P&{fZC9|2gyZV$ z)QNfiPi!4@r519AA_T~E7xgc+(w8w&FkMzw9+D)<5HOR>AF_q&@4tE6K6_X0=y!t2 z$i?Y4b8eQp;G>TnJC64BlJp3ZKXDVCU)a*AZ>DVbiEe=Y6(Q|aVr+v#geW_U*v;tB zXur8Dd+}TrzeCBDsrcR>!VOMEne)u?=H@*&Ct=p(W%urW{$l;}TVA6yd-TSJZNu?z zx%d0qYUbxB$A@WRo-qm2el!7BZ{A!sFFV|czB_TT(@ZPj%shSgsYz<9?)OtkSvU9Y zo}Ptr>!B*kU-xbB@J>ZjeN@fOcREofF5@jBLbSyGL4Vf%dEv`gL|Y|(021j%*QfCIs3wL&QzBqVK)RQh4h1(tP#B>)YKy`DBP0yQHgo>emjfh6K{soSS z&!xDXL4~=8>mzBr>z|A#YvL%{<2T0@R8@0y&|6ooUOA$zbz0Y+^4J!4-x_UhlJI3` z`@ts{In=39+>p%u%mA*>Z=+e<=Sf6;L%rveBfHY(+-vF#-8c%{Vbn45%4=-ywEEo@ zcQ)Tl5~3zQgA5^imamOj*S;$fv4|oY{cHQ|OJb8o-SusWqJ@Z|ZdNeOLzU2NW6S=O z_Wr%)*u;+?9ZWoQslL1?+S4$fQxmUs&xTdn7wT3L2aKeNhD=t+`upw@_}0Lz7G}O1 z{wkn+9Jn~fb3X%2bxOukAMPi6d3+64HI{Fxbj9=2X(DlH-iMmHVpcWH3JSKN5pKnj zuI+T!H^vhNMBF;M`VP&^t&06slYTV3!8@mW8a?H4dHzIl?@z|bubK11@qMQeGG=Rf zwFFllqK`!v$R#!9n-~$W9l@h!0L|dMbd;4*Tv{nLJ1cX%Qx3o9zN}h2zg2x~c~Bx2 zB<^^+IpHzwYi?mR?O(P+w z>m$+MDUCE0eky4Fy3Za8eHt;hXFU`;k&ztVwlD0GC}Bk$K_g)Pmd8-+)s1ETSB_(- zsZwuPrd_&L=5sFvxu}{9*!sZtsDEbsx4meya+>P%!IE}MQM-C|rM&HfK5PB@*A@rRe>P6@M z;NH81kGH4=Ua99$+)UEbW2O^VeERh2!H*yKc}C`ZxZFd2JAQBDn`@ts% zYiUUS`p%vnX0AJd;0_wa_C~K?zh++$fwXIA2*=0#=q!A&_A6huzQ(P4LPKbE!mn@s?+QZEP6&dDB&Vf5L@UP00*3 zRljAYR)3f*nshj(q4rEvD3R>;ZLysmJ}G<2B}#fv=4nrfvoja%A``WPPmCA8H_B<% zR2Jup8<%pcT@(~l+@jWzrFSb;5~Y?8Hz$jaPla8MrlTaw&`ns3u(_g~2pX5}qu*fL zRgR!aUbPZ7r16Ix7*u>nN?M)ryYO7*sq*)4Gx5BKP)aiG-AhTO}Pd zF@CYTQNGoPvt4)eGBcyIR1~&5S*TYyRuU*35^{VvT=?wGR+@;@`dfl;_EU8ox{}9B zH1UP{xJ{B7-jQ^*?!VC_5;K*X<5;hT>x9YL!)MoT^Ff4?eQf$LPoeF&)VqNxZh4^q zfbIDyjXqB%(}zm(v>6f`;!H%wAjrG^MaJULaO;=w-W@YtCRyILa2yGvMhm5;H~yzI z^F>v!MeVCHTiSDTvU$LZSsHYoa2gP5-HFd(caqAGNWN`6arJQ1V9p$#8=I7b$$yOx z4CTR&p(LSh*;#p|&kxlu`fUd?wiz4SafRJ9qWfG-l;c_e*xnUYq#ucxm0%!E3#4S^Z}lDjGZUaZmI{TU#z*DL|mrWe$wLvfzqvWwrJoH78c$Zt4(NZ zIAIqwbRiOqVlXk^VCCiYIP4X?Mz?)?=$AvoYUeLdz4?owie=GQ%8MF#Es2}Vow8KCZ8o*bE3wzO1{ zC8Yzro_Mj>e|EH9JRxpPYQ|NaHINvqr?(}UATe3Hw^}*9`+Yfs@+2uPIlV7oD7V~i z&+_7U!o_x~B7)V#GkX9knx5R%e~qHV{J=)SXOsUk(_N)K_iDSdO+osz@#zT#Q?=32 zQBa*MG;AMC_a`;4tBK3XcvN{`F4>I}wl=!8M=}-1&b#iS{V!8iU6wo|7G<7ZRnjT8 zY^`xSn_VkNdgN}K$%8jHH?eKDTWThDJ_c>^X7+_`jI|yb)lQg+E!uI|wIVPGP2OS; zU~LYX)o1Vb8szND1O}OP2{syzl6L2$K!lC4)_n~#T$tIn*)HMS!UMJl0klOJwWY>j-c%T^j~fKr0e<$O7$w%4b` z9VasCJDJFn3ZU9Fddp%e<{rfL)zJF}N<)a$0h9f*nwr{j63PU|Y9lTEL?}MyNqxz~ z6W49xg$i4bxi}pRsQO?~?{ISP1yIsQ!NI1>TayoT8&UyM>Fn!UVqox-j~rSpZ|$nP zr!<0aU?2{919H$!WQ`6~&>noEf!8?u^%s76y=_3fG zV9U$fn5vAzz$E|rOF#5)L1S-D$aHVAY}tC)TY@ig8k56Mf0XCDcP(esk|7>Hetdm% zI>SCq>sUS~j#)@*^9~t>uq=I-dUY726<+ zqa@}g^E6f`;WW)(zT6(jRf|?=j;XTWe+2F< zuf;xVe(yBziw1!BjfYNcG_p5k@5h%Y(#224QCKfuDsl1$mva2Y3qy?L!%q{79#`|hnXy1ub(H? zdX$RljhCvVFct9a(FXV1A+ktek4x``zxkKJs%NepY+9i8C{0VE>}0BYJD|i(L53`Q=4I zzhUg8FM3D~GU0cAU^>Q*qHiiU$fOU9>XQNZ(clnDymNla_k*gY%Io`y2Dq2! z>}=3AtqM6=8Pk)4c~)rP1YxJ-R-`M3AB8!J_cfc?hB?h44x zCg(eEyf=(uSr(OkEfHMDpV?A~9jSL<;6N7L6BSKSYq&_ZV({u%cjLY-CS(FspAn8C zM^@;aN|>WD)7X%ObkJ@qdb%6{s8m%}y!mobQE%(?%}5n8P0{n!usFIP>+@4Wvr3yHQ6l{Qz?kr zgm-amrQ^BRf#NSwJq$t1%HKOM*r7G08Mx}vLltWZ_bq?Bd|*3LQCs*uq+^tuk1w`5 zjFN9$U%Sviu3|mmB=m65z#;B<8i z{!da?B~12Jie;_kIA&k3!N8rlh0bG^W|7z;gJ*+-LrFKun!^*RsAXk;Hn>3>+sJ%p z-x+hbq@?*boO-5EaMm40ix)q}LGM*z*E>)a8?J64#^7dhUhbQr+$dxBq}u%5gm>3C z?&*n&iPWgydzFr$5v-Yan93pmP-J1x5H{h6Q-FluZC^D9>@>Bb!%>Y?d*CgPS?B~k z1+^k553uaDxVuk1>W!N_+RB6>7>m%q?!lAsLP>3_TL{JIX{p}<-e753q3 z@q_K5p!Y=7j*JrN#;rbA`_NwVp3%%YzYUCy@J=xzM7P>3a=R#WE zgB`1BH&HWuUdK#;KBOm@twR!!_v)O6A?SGwN506FRt{9!j{Q0}wy?1BPP65xvQ+7l z+!iZdCfHkX+DN+2eSg|}JK418(eBCd6Z_hLaeA-dOTSaQ%v$PD#hLi0nle`~=>!9o z_%dlTK74Q>t(l8KsQAHkycy^OeW(?U89n3zODG_uNye;)yUCR?eqzmlwVJ%@ zOMs+jD%ynnR%LMNz0lKlb@__Q?8j(zQTK#tqN~}}_5i&5Rd2aV)=%x$ZB;WqVs2TN z=zXGoK}G%t-;{WFXon(q?&E?jrSHgtdmhRRE+GrWirdq zkxy8F8xYcMvwc%d^Vu%u20XwReeAWZ^jU7X5A2Zu6AG>?&i<;KBlRgH5~txnt+~E3 z6cp%tG5|MgA5tChQdCxa2?!Dfb&8*&%a@z54(Wx|*QOSh3%08;sVy}-^gN>hahTDT zc;193l?dHaN6mFtBFAD9WI{TGtv41E%58h8-4mp7Us+IFD~GhJQi&%6!J^i%NYr+w ziQRwF3CPi5v8)+CQSj?TckhNM?M$DgAl|%DdQ?;xQez&~@I8o>te%CtUN}AesdyCo zQ0Tyb9O)s@9k!N<)=+4XzS!leTc`yiT-<~m-=Lvln}d6Rfv|4ZM(t5QnkiuLo5_;1 z*w1W{X-E@}b+GDVe#HEC7F2`N15|?NYq8v+&L4~20J`n_kfU&iiHTW;`S0jD`J9i2 z{CUGpMa}pZ@be5FOST<+M-Y{f$GcpPm)318Ov?e7BD)+RSmxGF3`hd^HOeKWUCoc6 zaNJOTuTXA-i{~=>;m-cnR{33JiROq*uYTEE;-c4Hs8UufO@(fp$dpg$l6xIC0dXT99MWZ(B{ z5MqCL6Th0EM(o@ho#UsQ81?gAoP~4s;O+rmlZy!`*!M&-man)WY{<_|vI7YHrj?p! z9UTF^GW0(feqd4?4>Yq<^Wl6SGE#aah1nDafAjNrPO;0K=HoH$kG}vq>p}5Z-Wr<% zIthhJ%3!`8pO@?ElQ+TmEE8@zA$@xB8Pk8`{x#_(i#dFfWCG~!?X^`2>mVW4hgMF@a6n8uymJZ69|Giz8sV z5V43hC=8Y-c>DgX<6`hlL*bWKyh&*P>MfEPN6ssI)mE1m&cW(^6yW>!6ZRh2k9+{H zaXl(>sqQLK*J5hAm_ugq+U0$dfoeX0ym6aeFLqbE{kQ_u(=E3%km)@EeF(-U$<-LI zz(D&ra+~OyZw}jj7u`SJO$fYr(qJ?DdWqAhD|*2kLM&V+?+SuKlDpOpQ$aKAOtdNe| zaD^{VC!8nUCdB;20JFkKK1r3X)(!O0*`;g(6{&YW@{PXdH0bidGy&{W}r6=3^i5@O5ex>yb;A60tROa2e;k>-%Wr@43lv@Wt#(evQ zkoD?1ST6AMTd1;GK*{griZ3oKRCx=d2#L9VO=TMEQf-XlEcmfVi;u|xUENuX9m!W- zR@dfwpC=lS+^qv3d1d>*!2NaYnoDKQeX2iw9(gS-qF304ICYBl)1#l?VoL|e1zuu# zNATc<^Gws2r9W&lv<=VGVY?aDA>2FbI8d*_N?(6!-*CDkonD1D_JVC2uNma(IE;K+ z)5mfyxCA=4PX2u@1S@-oRxLTxSWgSkj4D~+xgOK zG*0FD+bs8_w3p7e1D&lm;YI=bYiyiq>j@$B$?~*+{{ou^p~{>)kUn#by(0ke44ha% z11zIyQz+{sM0@u0ZuQwHY?{Zj>*EuGb}*@1p1TU@{#d{nSt4(1ZMLkHCj!OTun0tjt%U;hOfn<#qY1!#5*zahNPNXf9; zT?HE5ZetQ3*3r%Z%32H8p|Yy@Sf#ZzKt!UjZ5AN^!~BOqOBl1M ztz>YQk2s1kIo=jXJ*318ONT)WnC|r>>tDgt{5BsvjvjKLidlOPtjNf3evgpdPl0d$ zOhLy22@CwenpsCr-7WL;#mJBt!Ry1g>dVt2qy6p4l%0}?k<6^+FS;p^wd~^tf)1)S z`pLC&J3r0ZRoAzZmZuY7xvGnRGGiqQyRie&q}#{%PsR&fKoGq~xO48XLd=v^K8X&b zOf1+reBzwZd%pk~tBS8Xy;51-780!a6+}o8jd(I>MqplRsV7AaNEEcTB+G=Mbixv^EdcTaLJy#CCYg3H8sMfKe@dgBh5V<2UAbE1seSxx*^ zEVU_W(VN|Z(Nqg8t8!7aop3UioozMAMU2i`q7?toa-oB_v2DG$Ce#9oytU@i0NtJ< zy-fR)O7m>*CHJAU=xTor8%Pt1V;F zj?sfEER--}K|EZq88m2s%$Yf!0P6h${PHXaki@SOW*_*PF}kLdk5A_2@SE3EuCwp1 zGdv1TH>!=V>3u4jXH-DpAa?#pNxPWEy`6ZDl=LeD#nQ!(JNNGgD-^A@j{)c-GPS=h zSH9Pe_sL1~BWnGd_0J!^>8cVj`gah}+ICrF@wVGUOF_}x;2r5;y{ZpdGy*p=K1k_% zi2yumeT!TcR9GsXpR&2<5|yt8TjfSf8LD>G2&WCqxh42qbpj@~3!n-gZ3?i&=w>$M;c^J&PJJUv*MEfVQH z<>Q2Vy?9Y~f@;KoNz839Yy{pGF) zHh^DZIyN$IgUFVi-8>B{D7IV)z*0V4Wp&>iZ@+khM{tcOTWfFWoBhw%7xiA%WZx5r zZ_iz9AUV}D#XhMqP}i_*LDzm7e@_8XT(>tDh5?1}@yb}&{rk}e>ji(CnE8)_D*a5` z61k*qRmX;Sa;tRvE8jYft(--UH7wqOPVv{^KnZP)t>Bx+=_p|h4^PRWZ%9?NoEWBE zK81UcSZNGeMGc;pzS!987v5?KyWw|Eeq4(i!0cjxTta!@2M1frZtiI5iQe#_8NvF-)u~m8_}z3J z#T3_saRBgTNv!-0Y0Kdjpr6!o zkGW(MJsnpljz_1U%EHPT#1cWx1KN~;E8HZ%y}DiU2mHw_&@Kf>MmAin3ZEeO9*?sQ zN3cflz+zGgC#~Uji~Wq^d1V!^+7ff5aG;xt{_RY2VLUlwV0)qWL1_{r*UybKF>?}0 zqYaK?1-783CQ`R|(dEj{ii(*A@PFJM?1}wdW2;SjP0Xl@*tSH3R3x-MnI|V1A&|Q> zE6WF(6<&}qI07_@Y}kaMoAjnpjq^9yA`t>XEiFIzMvelTXFoYLzrP@sK9djm$HbDS zh;5|x4GnQI9M6v(cM(u?VH~9@51r^5UtBmWJ&2i}PU-@r_M=$ip_;In7;n+>$~4l$9~MS+DEZ)Yy3JYq)>(u0LN= zi6&I2;fP|a;@f7lQZzIDO`B2_9|&%xQq*ES?7}375FGdDaqK(&rJ?wSf9Do_U9f%r z>M0$@0>R%_=wdg9^9@HNcAP%5c)j$lI=pF?2slW9k!OC9?ptth$LP!M!%b#Z)}aD# z7&Z>}e^!j6=;GCW671i=gz>->4)tVcqS6SOh$ryu9G8$Hi<&13X#D8k0bbB5=-=+S zGplHdh$k<=`$xj;eNc7Vo$F>8u!+D}*=~nU+)0Q!F@HX1u-8dxC z4Bq>-qywy7pD9pJs>u+LPBykPDzav`FLI|m|K2NCYK8^^+MTAK5y%>3fgaSQ5J7NH zJPC1lzod`}oBZZY;gpS$`r3gCmC{nb;D070pRiv5KLXOvJ|=nF>;k(G5?>oXj2!!= z7!KS{3xx}=B1+#>{{C(sU0_qt!eQE`Zp#Hlkwqa?e4MI0NuzU|yy>crn!#@}GiA3K zff?qVIE27x|D6yIi*V~vK=eyNBS{UUCr2UL3*!IWT#ESLkW++P1kGZ(nSBpl`alL+ z5fgJ`@xXBNMh>?K;qKk~?m>Du4}YMFloBduIh?2W7q@OclTOq|Kgw>3)^%D#&1rJw z=h^R*^>e!Tf8s(uPMvc*1aVkpC7XjdoNn@LiUVP8%a|AJas`JH3yqb5Js!@Z(3Ut_ zBtZDQ^r;gD8n)&szKYIYB4)hrU~!TvoZ2yfCj2)-KQvm|-9e5CnbttDny7{dS2j!c zAvTmk-{>FGz9OpAj{L3o0vAih?C*F3%Flf-0AGKO)cXrWwm^GtvONrl+RGJWGAM#G3zS!z}RwuUj5Y9ifn6 z3dIM57wf`H1T&ISZ`+sNH;uO?CUgsG{x`(v@&t^LE#N+CFBgt@Q;3M&bEh^F!BX=M z@4upr#cH%pLSuH4U{&(pSeUFhfO<__)UG@8j_L4{9N$?ygXB>u@{kF}R3_xi;mp9E zEA&s|0)y(a0X8*G!ziSF%4T&lf6@q|X!!b!7u2o}y2CPa57lt9Epf&DUUz`oDm5+LbLX*u{bbjc?+4%T!P%fGIoJK#pZSa(Y;fHaCiAWAI<|0s5L5RG=L6uv1zjb&r6^~tKb1mJlHL6JY+ba@ zWUJ}%CwFf|lP2O!q*syxt@rd1*=RyRGl4^`V7rU-MPX;WP z5&H*(JJC*Ddrnf*1Ic%HpQj223Izawk0HZYSXjWH@y{Y=+1xKGqky0mUP_)&Ie%+9 zacFvlkoroC9CK<*lnaoDZW8*p-<^v<915EC91BL!p!Y%J=8~G4bnbut)%-a=`r zKnplJVz^|?a<}sjy7zaf{pR5M6lTgA&ISw+W*ivKGvt^ba*U%8krtpYMKWW@*vc<( z>u8M1<#FpfP7rIBVWO@bX4p(gzl+(N7^ zfi^KBWn3;S8=RpC9y{O9Gb7m%Qs29$@n(pJRk$^>Tq&UdL-T-ZZ`M)hj4tNCS}{{lCQ$7A zcxW(nK;N6x1Bn>>PNk9iAW)%`3iyN!0?>h%kkYn9>Wc`)EC9I*0__R}Y^x5pSTh#R z)g%vK^8aoet@mKR8l&bPnkic=O^X#c86@oLvZXzf;)pu9Z(5k~@+Mm!os{}i7jS-j{co|=fY`SPENMdeV(5t zrT<2mCL2hBl90}#WsL})pV>m&vh@Avk0Sp70T5=M8L-;q4z;5W;9;Svy5)iU zYD?te%Pz^EZq}bLT-cZg{%5!wwV6G349F`fUjmj5!-lBJ862^fani}6?XFDSNBEW)FC?}pT-+L7k4J@D$Xzo2*gmF$;Ft^n4Fqw=+Qp&%zx`3J=VW(dq7Wg8ElF+m z6ruxp6DUBx+`%lEbb50qkQBkhw44*SjszOU6{$w!`mZ#kbIbn%n!fAzRFO-i$iAzV z%WxXAA024{8y^6$r8Of15jc<_dDaw)fMOk?l(@A#%G?1wSJEz8NPhN(eP86CKuDrh>+{P3(P*zgB`#oG33y=SiarxMW@nU5&73F+i|=2kH3}Q`E;lP`$^yx#y*ha zU%v!Y^|wuXk&(~-unvI8?B64`3WBGrWBpKuZ<=TCUqJr6Xf&T@DJD}0?dZ&?EmVl! zukx3Cc_%tZ-Jq96{tz9Zr|v`2OY9c$<2>}+hGd%S%FOQsU`w!Yh-)NLzi9i z3NAftsal}%`d=LTMJoSUovb+kH>l0LAUsmy49(OymDrWkW`4L^2&6rG;|&aslmIri z>vcO%ma0jkfzK-ZVO!&4Ev(U`qzC7R2A1{b0;d(WP7V%(rC%_U4K8QoUNT5}zpx}& z{Xe_YmEg$ZC1Y_0Je8Uf50MVAXiGGJBTO{qTbP)%)NQ;(I@=?ic==*Q5H>fpE4cze zE)Qb`R={StNcCE6PLPqheA0ob7m#Uu!t2a+#`T20D!^zYudMhPSZ9HSv*1UOMx5B* z%p2Q@nm8kGG%w&uz{dnU!i~bw$)V>9WR{ofCbYsHB?A{qTMgqR&;BN8O)gNu1hMRl z`e6`31L!g(t$Db$f_>QAl)w`yJJWm~%Swd1f^a(r)AKAK*rj{U6Ja#qol42e(e;ra z6OTZ^9I>jPgUrC(qO(v1%o-1Y_zSt%50b<*t$a>lz`YAAw=8v@6alh1aMVVu4X+Mn z;s{33$hBA4?QJSaI=v1`_s6^+EioRq#L4{^nVAwM8AfGqxpURYrr7e>;DK;$ z{Cq=GTwgJQ(jy1TE)dey5)L=d=|ZPzmE>oZ(bLNbY#Q>S?u8vZ2Q-2?+lf4Jv`K$* zi~J&RLv5~mDdRlVgZgvWe1N_#4Fc9)tYREkP2XuYPfjgDAWsY@{z>Y+36|&Nw@z&1 zQRa_9WC9=eTj18ic-Me=l<7zFwTSU@f5~oCp;Ixp5uyuP=8l}G#{9S`00R6_=?>Gi z8o9fR`>R`TUp)pQSs7Ry?P8OD1Ze%y2~NyRQ5u%BxY?jASM#+XJx1Sxc@hlg_6~f3 zqsO4a4rgXobj#6U8u*uh{k*fI!?e4?PVDS#4|s-_L=S;nCfS3Ku)oI|K;plb3IU2CQy>BKbM09<-o zzJRo0L^GgCbPKkvx|RV?-no<01Cz?g#wi$sc#W&mwwi={k8h+H@VZWpE5>}FxC{e% zL(ieH_cM1R^U&Sj#vgCOrWB-1LzZO#p97b$+3`tlZr-|6dZi4U6}m|HugqtnCRv4}$5+IV2APWPQBvH=^2@^C|Mg_iH-OAl~1 zU~(OvENEq#ApvBz*C0LwE)HpUu2Igpis&_15|N0bY5n~V^qPUK^?chi$Gs``myDV2bR0fFY z9x}rG%PlQJ<=ET%h6|E}J$VZ(`&yD>aaw&|Xox-tUs*3M&RY44%+9(|4kO)xfl_(w zW^hDo1W$7SN5E|E&6JM;2=@q{B@WX<=#a8$JOemswW#9kFYu_4oVKTTzMV`xQe2&T zZSqvJ%zbl=DHg$TjTu@AWH)^;uS~+976m%B1(taGtMgNSpcN6-ctIUtLyYHt+HDeQ zOYra_l*0pfZFNBeLQMJ-qq+6Qi*>kx-ESf;=aiU!;cTyuW7n-WNphM_=~*#>qs|a` ztpK_0ny^ow7#Rgig|qzqFV0tUeFW^Ng;iNEQ+eM3>9eLCLK_@5-|K%lUhlX0^%=$z zbxc~S6G`>G4^E3E%|B{${c-8a$ zl=C(IzG(WkzZH4dXQR88+pUVH4OR8|Y{tvhCj42525vbr@-D2bQUW>F4G_dRU99{w zLoLv`d1OD~Vp6w9taF`vdfTN2wzi;JQv}?Vsn`miK;A^!k6n@CLly3VVHHl-RS#5C z)aD!05z950Z^R52Ox8a?Jy=^rz@X~tHv~PINzjyu57~f8DGZO;wYdF(?#zMPB7y)E zE?3qEVWfJE5>ZpWum)z^DQK15oD6Jx1S70IFidpl$({Cn}2@h7M*iaaf)j zJ7S?HoWs1&YxT;7*0TFwDPY=vz$4}9<+dJQJmwLRK=>+etgO6D_iR{4O!(Cehj0cH zp=So+pQ!n(jszw@{S_6KiA@wnlA8LZnAOM1^3v%0ySM4V`4IiuZvEOH(@w#++VxX> z=MgA-Vj==Iv2fO~tS o@j*FwswHh`|D1U8GMZcXAuzF$OwxJj-TV8+vrp8 zKvrDS1y#ZhYL4FK=LYV$^9hjPn#|5C2*~P;aZE=7u)f)il6Gtc)p{%{X3W^p5Ws8Hx{(O zSwaA9X{hjQ@gyF#8P40;-sEx@x5PH+rcV!+U-i(gSwb*ls5>1caE%kQ&ay_eK% z17cm!!JQRP+<*}Rv}~6b{pp5otJzy`EuMkqQD4H1ULn1znR>UqrLB-|;+ED@VS$Q) z#8LWKhlu?_pdmJ4ms0!!#wfH)zv3`3v@Yr@5<&oU`sOe>w(Lm@f(LCege*Hxl;ma) z1vx7&9rY>j)YC>^KdIC7(0D`d_ zl-kqz?|fzHRYJONPb7hR{shDH14S;a78aWt^TbGJc#Tx&+Lim@RM$$!&o7`)&E2>? z*$uptquw(R@560Y9yb442L~fz?%TX?J2i43J9}rxpqqrO=jP)Gt1cRETL)+5rmu1B zP|JZM3IM+Ov{BPjJO!N~VDHSz&aL%6pjT9OjS2}3-tid{KJA3_|t_T8Qqk68!)T>3@e18*YA9(`{)iu1QlvLi=9Lsqu zMIO@VlGt9Mm6HBMvqNzM9K$5hmg-?*GJf}~NCkipy;^Isl$T}-Dr&jzjt|=-8LSEl zIG;pu83jyC6*h9|zXiuLHrpa((Cc`qYdYfqM!_7$U&j|2_fk&#Z*K(}d;~`kvM5|m zFIsW%b^)pTkQCQi97f@UQW3nllI&^+P7~3J`q2iwz`;974d-O%B@K#V*Xn==M*vge zOr|u1Rz)>O+tINhy__gi;ZsfVa`Z11r&eA4J8o+qSB3IK{6MQCDVDX|x$SzYu<)YS zmeO{0u330#0U%6ECP&X}!j&8bo;P5jh4S1UlQPZblRSY=k+0JSF4qE&vs@ z&+^}%DjY5OE;D6TbJ39EutQdw1qn%S&~iYj0CYS-Qkx z+rJlej$n(&G|3{M_d~5n^!f>P+3LZuh-eTb4&g4x(;giB zF%0yJejmy)V3qi6>Yt#udb;Oz@~`KA;QOZY&SsN-I69CkSLCAFF$zv9mRPM-3Ht`(#qFWb7R4I zq~AZMFID9?$1aEqqy?`J-ZBhNLCfI=d9QSzQJ<^YA061+qM2S%B>gCElq!^;Tcpi1Wto z{>E4wcwdZ19Kae_nSj`X@JPF{TOw$T$EKK$KNJ`wcd6TfUO3V3t5tYdJ%hc5D!`n> z)UEO22gcMbx%&N2;p6_Zy3PXZ z2j}s~u`AF`W#}d_^d#s(X&{^(Y2h3E&SbT+{130=J%#w==HS5M#DVHF4HlNDDe%Yx zgmVixR)DoK`||{{TloYudrO?4TW29SUT7}O?BsdZGGPCmSO>>m*ZAIp6ze95jw^&~ z!>NJi1xpAV^8luPy>BmZKa77i9cc;#OSZgxyyv@hIlG)O*)Lx3o<>Hd%6lLuCznj4 z$8%6EwIktMA9P%ORLuKm2~i~!3$WmL+SlG#^l^dBU?koAD<7Y1LnW<$7%mq_!jcE_ zLed*XPiNuT=zo^H!J)ud_MUU&3Vz^QBPxMFM#chst}3m3?QwmB&B_WzgM(bRykQ$S zbP`MU@s*piSXVUNe>7}E8L>#qto;1t+phOBUyP3d%=8p^RjL@Oy~N)UVzJny~9=x zs3>e@#!&WTswb(h-el4)&@@)FMxW#Ug9m#DeEsReg`Rt<=&-P`Zr_WN=Ntr-#k!dc z$f7uAx9!2~ALG}3#Jnq<_jV^F+;@MmS?R4?m0O!`PjN+=NP3j)9f}U+7JT}&Az3Dw zBtGtWUoS0@>K+TT=6w^3u#iyaslKkh3g+hFs;aM?P;IhDwv+moV>Mch=PP%KYPTi$ zi#}iWo6!hrD-s#%fl)&s!`X_eygb*xj64iidHB zT?Kd9_-3Gc?YVvJq(rNIWw|G_&7<1Msr^jWbgFXe@d|d%9MECUPP}G-Z97@55pxTa z)iqo2}d*IIbNx#2# zTM{4F-kGyDO2GP;{H8%=Jaex>-H7Lnk&^BsA&;k)IrM-A#QBZ61tq0iiVpw648KdF%e^nWxgON9~7Td^hSx@`4f$A2=NK? zzoDqSw(ECE?!$suFw?eUGknO+)ckOl`P^~V|0}b5t+>n}ivlNCcsiIX6{6;^Bhgz@ z{NVr1n0@P5I5{0v^pja89pCsl?e%Ky>IT*Ss%;2^Yde!)Ew z^QMirFZG#+u5MCN(7|>wr>MJ-4*IF=)BJV|Tb>)#q@RIte;tX(p!sASX`|cN;J!Io z0KO%vw21uuy9OvJL#X#b?^b^Y(GN-w<*fRsM!IiJ7KqY&$pgZr({0)7%6?aR++MFYm{+*e`hW-pYe&my(i~35~k(-8zrd6bC1{ z8w;Zw&*2xo=*h80r!VH^3OYwhiNT3f2Weu}F&fxlIs>foV&V{AwewD@zs6;q#IzZF z#I5?67lW;|)pNX*!Yf%}a$E5b&Kd3V(+^?M3$~Q{XQlSY8d%pYToYXtkA)E)-~Hod zuj9?igA1z+Ydihqv-7h@HXS$WFM9Z>)op46TRF5proe!2v~OK;@DsFHODfM-k9+aB zok6F(N+_+r^JhL0&3-G&1iBYV*Y~-?A&TB>wb-At{GmN6Kknk7vXQMpflNV?PQSvU zow(PyM5Kq0r)wD+zKdvmF zmuVy2UyUOdJtu2j+0Zrf?@4r~wRK2F@L z=*{)B8mJA53ta;SxAv={9ym(56{x*#8!*a0|8jD0(50caI=gDhA+6!xug8!Au}&n7accm)n=ILqf7 zuhAQ9V%2$z1gZ3PZg6?ovvMPCPT4_cF3TeXgsZyMCPze^t%k!W}#`zXH;Y0T4ENEUH3+!BON==%E}y}&&>HTZ~M z^(~YF5p(Mnb$(uPz4j8~`F3Mtm-C-Z%lguS6MU$$@#q~8EAuz@mg6S!Vv|k2H>7c& z9~84cc<_-~_HB@kyG&E-;w{8I@VqQ{-;-DCu<7bPQFqPPlg4p>A3v{-#14?1qkg1r z_~xpPTg1p1f`g4ny+Mf_LDHZ2g55@G#G$rV6IrIiiFwt;XGrWF5m{N;uFhVB$khig z%SUg~Q&_tFKf1m;DylbXcMJ>=us}rt0~ARmr9q@cx=TcAhEhrzQQ;?uASvBBgv8LL zC<-_<3=PuVop&Gf`_{T^-TQ|wmut>@&UxRR&wlnkXL6K2C2%t{X8g`sh=|Alb3SHl zBpsM-*81q~ElD*^J>=p=71*cp`aD{q^3&M`_{G{3&0qPAxO zC30o`PwrDOR`HSD{v9z#(=LZ?U{szbQMF@DR4HHO-o0?!wEpes#+{fi^bh5%>k{n8 ze>1V7kvBkvNodaeIa8x?%SU!cQPYiR6F{#HT*LxaM+TDV?5no&rv+l>r5`+`SNS;EPnjG zP6S^agVXzV??ZH;y;k(G;)^x+h(ls3Tc)oqQqvo;ICqswDv}@%#C4Jha|EU5Pf<9p zcz8z<{6kF=)Z*!jh1m>;&m1=%pGkz;JVV?Y`#)N}dcT{QCY z4xo>SJ6@q=X?=U{R>tUBc3~R6sZmYMsWh*(EyQ&?*oTBmoN4 z6@4tb6bx_}^|}q@bx;Ezp;$kZpUG_QFxNlBjFyEW4o;@Nr-23CM8ltw7|Am;8Op*} z=$PMljW@xwuaCG&@2P280u{H)W1T1}+sU|E-7?18tl3JHmyq=qjX|wYvBs(f&My?T zJ7>|VNok~HxtuHJ>HiJ{V)$-p?v3aPDRwGcr;&3$?lP#I#{|tB2ja((~5M zcBg6-eNqY|w@{^w{^7$5ypQcTVr@5T@BCjkG9~`7ow80R&5N*xLNSuRMfNK)_QFz=<`>xTWu+34dsridhL45 zY)rCd`6e?X>B?Mt`#ZR{dr6EMs)vDhj8_OQhD}w;S7~ELmWu5? zewNh2dN;m)rIa}8HqOFi-oS*ub@NuoJw}-cHXH04kD;gU2?|1>4ibO&+>}h{P*__1B2_|KT>aR*WJMxbTY4n^Qo@&;>t30kdqpc18vsF@NhJl zT^z#b4CGAY<=fp9)CHPx9wYhF;i-2Pj^Y*?5La+nvAVk-yv+GSdpWH%OB*Zt(36Ue zF3qjXp4jhWwzb!6TWW~oWWYzufy_4;!gSX-o)L`x2Iiwhe@1xKK2EE5ZrX7o+9y-H z=v&T7UV;}2h)$q!Jmk&ucsT(pO8^-5p0Q zpb@0!u3M8Tr|c`_nvt1Sp#e+x=sHB#RxWx<9E{MGrWiG_$40tt7gX;36ke@!-I&@e zwCD&VM^?;4EQAOz+R4C~S3Jx8X&j#)RkHSgqvRVJHw|1Qch-C{E0x`J(&?P?gxBPP zNsB$W2A6?4-LlmwD|f7%zMH1s6tYg-Hn>ET^=U5Lmx`!_LdT|IY*R=h#Zr75vz8} zrQ33)&lRs;xRmwLCH;k(VRt2>xb2lV{|#pAs1y8;Nns1jBHeTz#Qzp;IoWD*N``yB!e= z=tnv>ekX*N|EsN9RSMtfrs*%&Y+ERkzhgg}Hg1bNngWgi3~=!owM&KCVJvMc7w%+S zXKPy3kXG!Cb9L_DoW{yf3G2BXInoh`&FjFlNsQ>ZC|aB#v)y?GhEZs5&I-5JYlY># zf-I&&4S^bpHAwZ{91{BVeSgpRtBFUDl9Doo8w7oarO33;myNL$7zcPI>fJYh1DHPp8M>H>*gzeH?uo4QowDgX9_M*jwjDW`y@t@((s8 z$4pGrZ4OT)O$V{7I`P=M<`7S(!oUswg`bZOLkY{+$o07*C%5586qFY~(~yvmw0Yb& zJUkOoZ!+~o-B(_q(us*W>;_<_0PiA5_<gO#Bt9krAre>pZp7K46en! z)Xn&U$Je+1bl>v*aIn|3io3J1K*%^u2-c>jX6u3&Sy{OS*#rc1Hma7qdPu2M-VMGt zG3~4{rnNDxQ?qX!$b}v+<3a`!06oNe(UmCIW?fLJQl%RmkZE=gc za!N{^qw?-A%RKFW4*$?v-CYTMLmq|PVDU^aPOtjj2gSf1nbOYxD~Q4eQavvH$@f!*VTobYr%fsm`)b0I;iNS zDA2Cj*OJw9{NbsB+{b#W>%>>t%U-#DqrJ?DK=E`W@GrKH$ zv(a`YubwB-=od)YMR2BPzS2vzJJ@IZ-Q-Xf$ZfH1PIUYvcB9`xJD3bvlM(zW==CQE zD$^W9JdWDD%%0{G{qu*|_kDVj$^dpR;KsD`LWL!*Hjkl~o6AB4`UzsBP+{JP(lWkW zAC;!&{(!A~Qk!eYKH$1BeQe9)NuoqLLD!+gb@u1RKpQw4Eblqw8kK7hj-gbaFKn;9 zRB8RA=G>W`6+E-)$$H^7!`OIazfd7e8?Zw!4E(zXg&1N8DY}c|mkUj@e`B^h=Zgk1 zl9oo~R=$-BxUPM_P~T9Wcl|Jh4E8^K9;I-inYv>qgQGtwnh{orDi%s*mnxhi$4x$7 zYw=8tt;`S`M4lv$5i%!~g7~tH4bz$vk)vaXlVA=2hRqA^IiG9&~~lI z6ziK75OIXCYGhC@lG|yfF)RO9Np{-#|4+(5;HnBAY)un zFdYd;(+Eq+$^@-?B^^ljSVA0_B}Y_jQq(Ic(^KL!QhU0}WmU0at3^^x@@R&N`ADtI zmHWdPITt7?k}G)zbxyx`(cb8_R1ONh*tXkV>8b^zz9{LCQ~{+PjEk`UpcKdt>7H3s+VoqLG+3m?kssePf z@%#7a(9dbX=Wbopt*RudbCt~jH=moOZD&5C7PeK(R;l1SP|*QQ$%N;~@-7sJCWTBw zM5MLcDw|RgTUF&Hxc(VQ3W^jH+b!A|q=j8qE3XUdLzde?-7&GXwS#GfFk?i2A7=O+ z^A;Q2hZt*WjX-gk3au-GEGpz}aTbM2ql0{7274z)K~ePeRIZS6YKFnjk0~YiCjW%a zhb=E&Afai>R``Pw`YF=aI8oR7HH8if1ywoQB$kpU3S0x!!wyl3D64aeg=FqC=H(KqLz} zrQk-?a(+I`DMYKPFC9i}A@)>W-0o{3W;9gPS?xb*!`u}Dh|;Zj(H+>-fsH;misV#G zH#SEr00}QV*b*S79b$Z*oZ7%JA(9K>KZ(*G8M>sD3~3o3vr4UFZrhuL$Zf~z{436d~3Yj%;Occ zz3mrqpS_9w{Zt2c<~i{B^0A$*VfuyF5AB~mB3soI4*(ZbtK3R4qd9k#Y3B_&JZDHU zzWTM#LPrDVVD2kmyJ#E#6{E_-xW7R~MI((!UN|ek^C;n@IX9`(ra=*HqPNKu@C2FY z80$Ip*Qc?Jzv^=T-A6=oX6qV7)Df=NQx#_U-I>3R=!7f`x@o7b&)&yhkn~s6Q3@n_ zuh9K4G116<=t!FYzo0HwXtUb$5o{RB<%m(%g!ZU$VMdQKiehBhEsNqx`iZq^%@CsE zo8CO;&86WSaw|P@kuZJ1K_dY2$1xlsLzX<{Q3iE!+3|-m#;${fYkd!VJF> zo~JPq1{`8{j~&%92A$U0u4kg8M@-QW{x52gNQVn)C*1q+8c++(zo^CkI^6AzDlTrW zj%lfCBhBUIhI&57R?Y<>BcEp@BP>#HGbkZAZyc!&A3sUR9{YmKx<-Yt4z??-DTdQ&BAR63}@_0K~iAPM}g za-QQt(xfAhe$pR9p3y=BIc@sqv3&F+$zhLI;ufuugCnb7dHMRrjcE?M46_)!Wjb#^ zL>mm}4X26gL|;j&F1l#~ugsd7ns|b@(jt<(DH;<|G}$I8S7w)g{Twm?WufX69(ckmt`~UjZV(W{SFiXM*cfD)=u#=x5?Z9meU=$hKZBM7C`=Gm; zIrW!nueORW&?9eKd=ZfwI*Z+55`dm8A{?sbxB)%^n-x&O)}eh`Tbt-yA!jyNxbkLNBe$&~*&0iqdHFinsr6quVuxF4+d0`!vi*IvnsgQhGx zVubcJz~p9&Mar$8qz%UVAE*qUhayDj3~Sqil|=)Kt!XBbqleBzWtZe4YN##mNld#} zlQ0jy7l4E5A}DDvq4FVjOM4N zjTLXc%7}3oxZ7>cn!j@8ik7fV%0Gpnf*1E_;=?)Q6uPeXnZ-PC;`H|UlhC)-LGZ+j*V#43GXC!Cy44A_5r8Gr@tN zZ~gPg`tp3`25;66HWSTa0ABF?Rs-r~b++S6{COp1xfK8^((@(#|2eWdhd3#Xw^+mA zs~XO1)v8Lq57*2@ufGZcX$3bv&CbNi+`h3;ZYcNnAl@JcVPnstn{lDHC~{oTvPyUV zlIvUQVjY9~_ooHyGtDY%?gdvm7yT0%;=?Dp3yahj4RYgW3em}(_Rec3J=GD2AYM^1 zTg+QbxbmQq>os!FuZ|!7zMIn_8?er25gVmDmVlRBKUc1(V49|MTb>(_BxB0X>)<5d z_a1lR5TN(NZq&7`?V)3B<=}sZ`5eGG4)gRw)b#fo0p*(C)(A|)5f~kBGVk7v!#9x% z#tl>}e!Ze_;qyQww;Fo_RaWGEF&JjZ$`OfA8E_adgMH>W@Icu`KWO0`Tt8lOS z`&R9?5Vkg1b7(8~-|!%2ml;f_I2XTdH=}itsPeWjzxGvQJUhPk3{?`}(h^uhHdA5S zG_Y;DaCfy_hr_xF&tE?#EES=PE|6-y<~xiETaHSCg8j9NNxiopD?AnP`0;6oiQ8rA z9Rw@7#`~h8)Ax;hxLyxk*gbZv0|vOXxe!{D>mOMEbFpZ41f#uN%OGt*zs7aOFwWqz zqngRlzWZkkgOW}JSr-49^55UH@{^Op@tt>hBx;$>?Xn(j7IV-1v!lFXTp$9&mfKr| zh1dLP&L9^qU_jyvj|d-n3j~v&3oO+Bhsg)WRDK0ao+Wbpx8i(vJ}U1@#fxochFo_tqI7#|Ul_f*&+ai)0 zz6j4PMG6(wKM*IOXR)HLKjk9f)$n_KA~pNM9j)L~l?>M(Xh@Izj2!0&yd50KUpvkhH3B10@A;^@iSHv-a=j4ys)Zgc7NB&x4`dAjVeMUt-HA$-K+S-Nbaf8Y~2*cWjP+r zfD_hi3<5gu6D1?ZBdfM$hp5QpE0p;qjZX_##>c!tJ`Enj&g=(gYqvL1*sZ$((e!4X_ zu>g_VP%Bc7S&0#pTLIbD3P8#x9vz4BW6;Yp$iT+baOfZ2wyVHO6hVkY*t_i5Q}-8u zlpsfUh(9m7ka=^L@@iy4>`t7n+TbZ7@`G!#S#+(Z;67ec7eTx>X^;p0*<0m=V<)3# z$MXY~g~-gKA&k?l+^o=HOvZ>t$Xt{PSyBzL?yt&8E!8?|>FLv-z;mH;djqDHjCVA+p`8?iB$*chC{%_Za zkcW`$UEG=o1ds2F?oz;)IutRSvo`7>r_LDq?^KwkfnH2&sr3xsOX^>fx^h=K$}pd(UUm)`jh6aQ1{QY0D`=nQpSQTZG;!A?ReBL2(Ajn{(SN#% z*3H!JA{;?1f;TNn&8u~AKao*Vsqun6Wt2DfrSzHU>*mzwu}L8^8^W-&b8xMFk}R+z zuwoODGfwi56(-_}Dr&C1iK&9g8LnJ;vzN$Jp=5}X_-0nN(HGa5!L)vHL5|m@AWpnd7SFTtZV>0-U zD~L)=@dKGprOHy*H=#PnLc3T2%2%xU&1no=3;n$bk3`1M5rGMil|_taX1`;D5nAs` zL5{SQ0>5=f0w?6v-Fhu@lgqW_f#Wb^&z3e#Iwr_gD-ec4+0AS&5ubVbT9|ckJg1T! zk5plH9fiMYcbOjs3FU3~f}>+BMX$rlqB>r?cQ_rXxhK&xw@wx~eZ^}jtHaSF4?xK} zFk6=$_SW2EAG{o0c!nVq@|_oGZjp-&elg@>HW0&dcF3sb%(XNm5uM;*kIElL?AI{K zTItFYk|4)Q=w5JrfC;5bw|VFZPd;U?7CO$Nl>}s(nXbAZV&XF@Fw(P~Pe_kIPL=e9 zICGSlfuWhFA=GRvf~&`UGN2Kx$uFSV`1va5j6Nrw?5{%_x4$qW51ag)Hm((jF6}Ae zBw>UHVbNJX8p1F@U?T%)C|^^Pk&#smXh$%kIB8y5T88?8!jb)bOO8fpOQaTZQVHaA z>bj?^oZ3iV#ReNKFQz#|f2O93&Mc^d*F*OHnOpi(Q|2A7nF{Oh89fs3-y^>II;nyk z5@Ea;mD*dJFJoQ7+e|PTwbBtS-(O@3b=@yVrX5Y}4Gle&bN)jV&8Yf8w)N{R$pY(c zcGsmfg41ricYB883VTioF!}_ya@?o7-d|u{`JHcKxA^Ov>&A$`6O6oGO9ckpPG?rM z|IW@1259Gv$10p0W>NsBh{%R9Gpn&1gX%yuq`ND4EAe;UI;O_j%^jxZGA&5{v7H^2 z4vT$H7v1>Vwz1T9c9I#|fBL4PdY#u=VQg0WED=eRzQfR>0X3ifV>T!qG z5{P+nN?AD^Y|R>qTsH|iW95zS*@}A z4_szH(?DAw%r-5ns3o82oQgMVixRhZtP(sc=sX>oYuEvt@{afKKhaV1y6IFo?!L(C z9o^np)4O}Ou@}qAr_mU0bKqtIOb|?4YyEDFnvTXAwCN{g!;t2UIewt@Fy*>B(+4vK z%DT}2(FkjlZ){m*`6thaRovn|kU4p++(7~M_vXtQ6S#pjmrQm!PC8MN$*vlO!*tYd z_4V(pPqn1RD18;*2?UH`lUNNgnzifiNK_B)Zn~;0_={YIa|*bJ2?_1?i%%`Kmu17Y zyfpLM&Dt5dcQ-3`IvBSe9Ket`JHlvP2F1BQF6LVqs@p5V`|+SFW258KSaDxYgYT!p za)d)Z#aC6W2IlKr>#VApuiCSJ=lza1Hb`F~E^NR>8Br1l0QS~?Tv}@+VM|kcoMd$ zBO_}~W^KW4+Xc0I!Y+Fjmj$PZt?@zI@v9j(v z#TKf++6j`UlLm;{-DT(qY2~_Bn;nQnny}!?hnFvH?11h;63S|qJqqyQBKG=?+H53eztethz1FfpLlzGZ#~9Y$45(gIyx|c->S6E zaZsbAbrnb37?GSm(I_3LbpFDc_s+b{U379RS;uaFd%6tuJ&{)VUJ@5>tC*Fgak+>z z(f-mvx1ML$fh5~D10QJ+4ZLU7kB0aPB%ZGp+_qZ$S{+C{Jz?5%m-s`6k7Zbu#jLVEiJp&dL;uoLbya zZ(`N0$17g}_0g%!@#O;Eo1v0&2Yf`sukyQSK=MX9i)%9^5gu%ZA1F`7tc|&n+q_qXUCu21p^S5Gxv|0hL2MyO=T! zM*mA-<$F#{pqHf%grLY?a6E3j8zU{V&rgolw7T#DHD#wf}IEgR~Ii2BOxrPx!CLh(Z zb+N$5WM%S1b{48UC=tIR!=-0!nc@or%jCp)*GZrHQ4;j9zns===Qm$W{QhHYx|bT8 zF_`0KWWe5Jyi&HT`&A~V0FdXYmz(P1$4?aRtCp+3%%f!0zCD%d$(=2F%;>o>pZcQx zT$f^Cd#)}&(TU?}abFa`^m6WR&y0ar+UvmuCULW9F%;U*G3{5fN-7ZTU)$gGWn10e zS-^J5iC(=xk-V8A*O6so0>*i!TU|fP2O;j!y!o4$2}|=x*N1WT-aEs+_0Z3#sCQ;# zw#RN$m#2SJ=k)R5JAfYSxbL=bTDksFv8z_!+}dG!v1;nN(Su=K$s(al+X=o)T=F~N ztUB9G=HHHG4lncetp{Dz?JDMVOBt#M_1{54igT-wfFQiX%&bv0O_$#;knY{_lT7h) zSF;AoHLn(#E53MJJnyc7|+USM$fzj`e=nE{fxR;h# zdmp=~Pd7Pr_5(<+|Ko_^VJ%Qv>&}#`bQ`?|V3xs)2s6t$BdMQ>kXf?TIJoy@q4Bc; zq=O#)gRZFwB+mCjPhoYn{mx&2=v^aq?~4I(>11;nV7wL)96u8tRn7s|HLA!#zrcMi zI4}zo%9U1g_42dr%tOLI_Zq7S7-8s&81&N01g zrOkE+nVgS$pop}E-$m1#9mI<@_HJF8BL{}p5EJCFgZHGP0R+8mmdT~)A-f$yH^H7H zU$tFtt3hIY-$DC&!F2CNhEVC5R~Z0)(iih;SdGkfr|6^Ors)f<7C`f=k0i$m>CUe} zx5%Nhe?JjRa363VEID6TFi=+)=*IUJ(+0J($k5x;(=QE3-kz_EcH5sB>?z}&@-YRBBmyE&j8n*TZ5d1Hz+DY`a*E&sUe%B;uWUGkM+>`Z^0%UrhSa+{_;9%*UZT zsQ=))9Kt+CE$RZQ;z5O(2&9L&xa>C?@b2xozP%XYMbdKwoy5?+?eKAc#Xw6fuk5Ux zj0nxtNk4Ggj3@T>dEC@nU=gY3GD14BcRxLSxhl;zEC~#BqhS9f7XA4s{qnrdrluy% ziwjIR*XoW?9L(m1DC35-47_owEJb!0)+|wixOh>}cw*FaYGbg>bpU!J_NKy3Y`K{l zcpLB|$oEHwBD;Doq7)40Y>SA1?z|`JL3JBO*UlB2($A$s+Ge|A%vG!~6z^m^*DlTT z@mjjPaZ@NAGVxxk#0iJQrZ8`t@eYsrRc@_z?OGZ+Rb41W6(dpPeS?u5d_!P{!Rz{DA#U!GKXB)MU!y) z%$auRgXnOn37|tFX0x67ewinaRkD6bCCBn5gCc7^z04OGlHAkM!e=ud1(bc;eMa>) zP{~(}hn5hrc+~x+9h_EzPdxw)h!~3i#~&wllDfirQ{{aDuMGo+Wf2JoOr`$Y2ik=X zHQ5}mwTLs>HS3)|Pj*`Wtr_}_&?Y^mW&g+$` z@G7S=a`N>gsKnf0xYz9XKm%CmVJ>bEPw`$Odl#BNF%9%j&Nv4vxmXTK;Y{t+pwuV4 zw9tR0gx!4A&xsBW9*A)x%!MZQA!G-YGSSfr(!`kh9ooP*$(( zW-TC~%*>CO)N(r4`^!QgCM|^{nk}*M$ySvmw5N~kZgo_RN4(6TfC!ub#oXAY*c`U1 zEeWLgL)0K11|GK4u|CO);A(5$-;37fgOAxj*^De83Gf3{<$nO3E8LnZUd-)oeV}n> zQ_~k(UtoX;V`_U$0?@jUY@+l@l&L{p1RM1697J(veQR^b{Oz5=!?06${eh%0a(#ig^Zv^7S4MOps z8ZOICYl=&km>_QM`1t-sS?MdtKB~hH2U~AoX8$;?TLsF&o0{mpPrh@0=zi$K$R6;ZDcBBD8A1wBgkZ zK_B}w_%0H$FkH5_gRk{*K3jZ>WPi}Yi|!~dkvKz>3Gf%H>1Z2MJvP#)7r2>481A63*ig+<8d93zb5vy}ZLNptCm9iDM;2 z2Oq<9GHmqmhG_ziFk)jCYAV(F?LY{%16bpIG{5dR+~0Iaj9fD~WBVGPlS*WvRoJ!N z#-85p-QD(5M;k7y*6-5{aHo)ud7U+Y3{~`79%!`&kR(zBZvMW7&bUP97plT^@45(& zKQ*=*wSW1WynS)^(RjxkJz(H^i=s%GZX~kV+s62wSFLbJkte{tl-svIL;gSw z<7%caAHEG#RfZp)KGFUPXa?#vtK&peXbs=1Ol>!WNT-iKd z*1;@Cm}5^A@3esNhf$-Qw_(u5107Ensm?z$xT$i|T^KlqEn?Etd>YiC4sopre9Amw zN%9tAVkY6jIuxt6_e>69TQgs0-BJ(&cd|^~8PzIONik4>$vXe13k8v3R52d{e0)&> z)&N8@Ps&0`YNXc-L6)0R?FtZYpu=Pi42>|I<1mAF6hj7 zle^PsoPo6ySUoHH_rL2X`6_iOOE`Qeq?hnGl>xI^ZS5T691I3g)K5_*b_&46L zNr2-yd~*;Qf_@-0ePE}BX#!)N%3%%-da}bg-7XE~CN6v%py-~QVG}SiUg>b#C!l^( zOfojh2O&8AZ&fDvK-GimFbuC~aNB)R9}MK8zhStB>4FbMiYD}RTs(UX3Io;7CEB`W zz%dz{7}W?!dA1K2^Zv-BN2ogeNtv9)wn;6=Tc6Ky$_N_uXU z?_NBmci8FR24ayXoreR4!!!T@d|hLx?)2>Ac_`@_X%A1!M88lQX4Tz(pniVrswjaWWx?5QRo@|Dp~!-GZ76k$@ECr!b04K($u zokG0RAHSpjl{4oq7BiTb1`2_qJ6uY$F`R?1 z(8$;9i6)5(ivt$3!K|OXk}9tqFmAGdk4opcd)PlDjYPBP!iAj$Ncwb)@4tBA^B$_0 z=Z3CotXz=WaJ5YJXMd!};*%HrD-o z6yswhzkyh#TIK2t)uT%KJUv0KnSp-JoW?h`hvM#$cldhv0Scza`Q!a%k!stp4b*+t zIPKwjs0itdZgCo_f!IbGAMr*TB2SXp zy%0ah(ntNYy;$}V5QMb@PApo?d7b$L)Vc=dI~>Z+8AEaR4`nWH7*W=aZ*y)qa5Ne> z>DZN6c2@7>d77fN({scQyS{XgvX@9mgMh8nm6PP9zs6EIbgt3@G9W@TnkFwcGijgd>}1BNu#@R z6Q2zaBOC#4^>3nvuu%BG=Azg?#fW=`WRVV*#xqz!B8B^`3k2}e5D!ve^WvyVdUHZq z&Z1^;@^W1GLRO8q;e-UH%G-aRV+<|?&RPRK3j>$d4YNXov$o}(lplEU^4U1;+K;MF zpT_<8q|ouQF{&jOA1Sx886yamEv~@+&B+Gg>(xf~`>SR}RZr7T9q!hy`}(|q!1VO| z!2#yB^U>qyJI}61AHRtSz#D|cnKPPZ^$e(LV^tTlch*e$i_p3moymoTxI#(?dryyV zsJw>r*Jrh_zhz-QeZ!f(N;s3H#^lrmsqx>BWBg}^m+N<2#z*N-*Zp>GoVccQB?2c} zJ!SjxlWj;+%ac@_fVL@S_UO}#@MRZcDYU(qRuAUDX~#m2T6n;g4x-5p?AXha1&8w5k5w29=G>#_+etS5lSEkmmzs5GFgOm9|M3i!CD`dUG8S~5E zZ8g?k*4L+C8c?F?;Je#{!53Y(e{=Aa8+w0zdkTUL2svP4G(ih(vAqf{bQ}REs(;vC z71Y8W>^rE2h8?8Z&yPwQWog>9ge-D`eL5vXVPq7s;Vo>6{~gYk=Umi#wTQb+7vEE) zJ~)%z_STzLP2<3tH6hl4#;J>-HEAQaO` z$+TVt(!HI{cv^WuyX5I}{~@%-Gzv#vUcTt)Iko)mIdp$cZ2TisMqqGgieN+NT^_ne zS8v@4xpe*}wE#oKl2_8g)jUIQQstTxq^n;|V)rIq9BzyY$$+zbECBV4jb#lfjFKDk z9acG2dKaLB>w^2-OZ2r4ih${((|UcRtr=ve#b5 z2&x+r7CdP!;gvi9L{S^5EMXkR z1sD-mJS8Ba3e&!V?&!HW{yt*4bPC-a$(=szDYs$>KDzis%y(3pOgm$8GK)2^sqzGja_Y3ZveIfDl--&O zEx5LMhpHYtWv=GE_VJ$d3|;bWnQiqSYulH6(H*_!Cl-&uq_x{q`C}MDEqqTy?Z9RQ zU%FR&Rqp3QmIq&q{gOMq{8o-s;p`?F8=PL_EC{S?vz=L#z=HAUQBl;(=I|z;86`S- zR6jdm*F})$Wzx5sH_^{4MJ}GF(wcL1ulVFJ`?=C-L)Ud}z6>1|5XZW=ovyqv;4{t~ z;BkTKTznz0ATezz(=#r?Cvg*6dqq&tv!Is?TrK&;Dbg_?a&e{RWLsOC-OAh3#N;F# zmrvuqw*7eqzHl=&@uxp_sIR2KX*Sz+2mOE)qI0E?u`$L6j!H=W+z)gJX;h4R(C|+NosMn(N#TcQYkwO=&E)mb&i;foN7c2w1!h2#CsDOk+4^hSHIV{Qd}v zgXsmCnOgofB|6ism98wwkabKj2|24t7Fo2tuW+_QAGk#eSc{mD(W;G6^x4xYdKWRD zx(%QD>ENTI92alLs*gQDbjt4;$?@x7fEfj2O`r7IFJ!M*;-bM%fPOE(d}&L^q09?a zcq!wQC$xNY9g9cwT9{K#X#m9HgK3vqLgK&oC9B7VcpiTH)^51F$(R$5n5_HB6OB`F zQSv~FyE{DeE6nuflGAdBdPU^`#J12>3q?hBmXxEYxR2H)wk}OHW99>yx5C5u0>jh4 zwWVU#lzV3Fe{hH{>mLs{muxhbDzNF^OtfXXuX(`NbbQF&Tl) z>1Br)MDE7e?mSDDka{vvL&JzZkL>mU(1 zxGSt-d7WAL!QDCVJbup%*M45oZ~w_*GW|wgSapjCJ4%1IFyY~zuWv_=CdaYU;By6# z#=&er(M_O}eQZ=`{}Fd26_iRSrKQoWxg+jW`C)u%lS|BVEaS1Q`4XROoJPCeE7=-`~9p z(MB38oVxZMd9|L!KL~DWPx~OwkA(79Xi%aT1qE6|@kV6Q?^RnIkWy@+nS57iKi9CQ z;`w`f7QM`%lVm!b*KQaXpnH;iwdm+w4`7(3IJc=nzUV;~0Y^Y@%DZR$fH_8UKgr;0 zPvG!yluAUQAH;oGL7~fZ`%jQ}4y7x1Oq9-CJV&itQ+M`1Q_vOk+faVD+wj) zpe4;fj1)-Y$F^5nhTa~>J5S#&TH3>^r7yPM70z4-eIr*v_pKx7O_z?UytQq$dY(6p2N=EPJibf4zmk=I+^p6t@tgXTT1&GAUr zfX1rm-;LS!`P}g~CWv&Y18Eb+VWaO}IZAOT{SSuXH5({Ti40*PBQid3zKZ5wc&BY7 z{;rTsCrzka1Q1Ol)-XiU@%hG6&0eH0Rn^pH{@h<$Lc{M1_jZ_a9jxAoOLQtJEUd=f z6Ycjp(^BQO0z5>1bt8KEP|U-cnhX{5|@;@Es3)*>g&YW^Vw)E$RmQs=@K8;br`mQNYLd?0v>QJbA ziRDR;_$gW2Q?78>-sbO}`3amuu9+THi~asMi(zkXyH0=KHa3WALMSNeyK%B7i_BYE zDx5nCbop^pEn=sm`FCu8O5r%w)R5{2w5=uOFDOlMPk^a zY6|{h%jmJuO$lhLKH`1$h-myySMK)wh=2cn^aL$$g4k7_4Aq5!6G2B%L;MpjZE%k3 z=0=T?sl)ju70vn{=lzZP_8vb#Zu)j5#$jHK4 z*v`K?mH1jY#k4v}+Q+Q@mk9fD_6UeBne3reOV*F&tjCaIl_m#nx>p_2HXE!-VHwud}~z27MOkYJaD--n3sIw zNqB-Nbdx!Hlbr~<(cA)QtpgPpBK4)z zZ_26r%2;ML0`O@%oa-;Xfiq9zO%@mL5pu6&H*QqfZ%GVu^T4No1kPk!wN!W}L+o5a zxRDhUw8l^F&3sJV2ky7iDP|GWq>LKaQd8=s6-v8wAraUhpJm@ry{N9a5Ul39{fnkP z=koQ>U1AU0Emlpl4n{8&IT<^ZIt|)hV0&@|b%f=h>%K({uX68rle(F~ANm|}zyA~n ziiyGOj(Jq{x3{m~@F?|EZ#~jDWt$D(9rC5~+jcO+C3T;;bwt%swe`648cf4n9f6*J zTC>kbnw0Y)+w~X<=Csf3bl*gUsUCX$E(7B$iaYNl` zj6TQzy|1$W=?lREmQgpaaxRKKMA?zzd#(@J@=5Yv`&bzm!5iJ^n6iSp1S2OoAYxou zN|ZcU4}E`u*7U2y&XJt!JnDC#zQ)#Lj&93|4HrS7Ro?)24+eWt_k8)96~k*qgV+I) zx`w)z`l?-V=c+Z^{0!9*GHTn7(KI#=W&2FU-V#jBkEPnlr1GAj{`30 zfKU0#{tRadQ zWHe`cn07s}VySjvBasM28wb9n*gOV+cEl7ZV1%vX5L$3cILAS{>h4Zlt{zYS^LxN! zxugzJB_8}unOPgN_lOM--+(34S#TD3{$jmdGC2^H_|`59>(vo-Q-5u*xk5j7-=R{3M&Tn> z)l^2+e9fCh@TGPf!KXZO;04WUCSuE|Iqh%rYkTupsWrq1lHNXs8mjteIhbvAuzH8O zzc8zQ26Qi@oqt|o!l354iLe&RYf1SO%32fRwdNEneXv*%`fMthKlD8Bysly|;iWCe zAsCH>bj7=glDOp@_l}rkS!_A00HchnWr?`DTudf-Z`$Nz9vO#EM=9^^n@e<5!2#jZ znsv_WR_grN7-+~!M4@iU5EHNV|D?sNe7KU4HoSN*vOgO>og3oxlN#wO@;-fJs1wVAuoud70~#Uo6FePup^(9kolPZ5{pv(XdPnUrX%W?Us{Co ztIr5+Oa0i|Qq}7%)fXnM+_C(DGaE^?MJWg(cD!TUjO6Wjf9c&k(PQwEpUdijS(y3#r$$xp(qLiMiM`zkbbqS9f8deovBOBQFp~?z z%mom~bj-Y&S*Y<`dL}b+=xgCI^#rFTiTsco9*2j{I7$-Hm5Nuj4d$M(DA_Y|Uy_{dnrG+*SBSq+${ zw`CGd#{i`h-JPF0X7X@+cMEwRoH3X^t>y7ZMn0SI1j1X1N0W%vfoU2PloVq%a4Vx% z+IRnEQ5ZVc8)jZf(PZ9lZVT)J&HFNo+2Ar4#%($1U{8N<#Ir-=)EOk3OUV44Mfrai zd+VsKzU^Ij11cDl3P^{7gn&UwOGrsdNh(OUlr$@lbX!Yvzhx1}2hH^2SD(~jv~$tWS$8t5#DIw29tJsl+L&WB zpda@xi_7mnaL?qHb1jBTspiXEp-sc_ju9{BS$C;wen;K&JAV|T-!VP6G`}&GRy29n zgF`Ce;UMI zK6<40793S1AI(C}-c+dQTZS1r-!rR3~c6##eSUN|e-ywLHUSU+jSpx(D!qEhsDq{-i zvKKGnL&fe))|;gP3n&w+4P{sX?xBT?Nr5;y!}#wLBCcMYpD(kDavkYaMNf2WsJI_1 zTyI*ggj6FR?+8N} z!89kJOIvUj+gnaXR;~x8r;V9c!obnJJl(iTV{u%0i0mQp05Q_qn|L42fj(_kBKUWRDZrijEm0{B_)@HF<0MzcxmalKd6V z-lUFuPKtu@&%BVTyxWly$nP9qzJ_`^B_B38POKI}Oqsfdi&%(ngHU|wU z{s!s)pt)>7NB`f0ZG8Q--lA~J?$9$NAXI&mN&b=1E3%^gJT*zPOl@{l3;W?cq}N2u zUtt#5P|pLy%~^739ArK6irdb7vD;c-gX5B5DDR_3z4A7-sptWpdX5lX124{(-m{2Sk8MRFf*6oS91R#l*xMTE7KcxOnj%)p@m{>aPc$N{6p-o)F{Lvn7;?@s%0+ zDXfl|z{Vt%t!S1=>z}NhvYyT>hfSeQB%*vmT_=`DSm7M<>W#4#5x^S)8Jrs-(Y+l2 zaAM@oyqtC5=LNog>_@oG@n#$EHUK$!DKQe(mX?T8V86zntv^K zZE4eGS^S z0tz-3=1zz>uqQ}w1k??-X5J9$l_-zo$dSo=?CL^{QNl-^0cl=~?22~nunOohkUkO` zl2ovh)Pzq7tB1Ei^kHw%LF2JekAu?Cc7iUWeDa>hj(t<2&!F^$y2zkL?XnC$&W>DC zQu54xj{`Wgbnh6`Q=gbifvAs_trbpoeI!2+F~)*h?=C3D7X|zJ_*r~wBy5GJUick) z3(4U5_aF~8bxrf!!)-$^86-N@ESkf7H2F-p)Zp}$lA@Hxq0{t{ zl5!I^wHz0m@WXt3WbZ$MnLIJ(HWo+ga+(uEpdLP=&Ov!X95 zGMn)V>5e$`^@@B{p?kn}1!I<^L{zeU&h*&XzIy!R3BUbbfp(j5dG=WSEA%Uw+NvEN zyH`8$S9;ZHduY2>og4Eg2jfE&|AtZb#7WWC&QqG#ra$VcPdt89G~^z433*M)W`Pse z_!|%Ajdzp1HDVHBPv1q`yLcsUmjAvlS&GY8+@@W!^l+LXe3 zCh`_+otESG6i4d~bM|vtB{{#IQtAA7TwI3U$C2?sSCE=3Dmwj`O&tvzd=m}ZOWeoF ze!$>G7~Q%ds52nE;p3|C@hKgE0kC@<{USdt{ls*nW@_iym7h~lDvXvl@+{d85m!|R zr&^2D=kV{s%N7K}Okrw2g3{jd_IL~MHk0oEV~a{pl*5f~!6}w^Utny%3zedWU`TX7 zH?xM*pf_Ic0~5FlJYyM9L75Mclk%rS*@mN zz6@|yeAPGb zRT*wPo+p(Uk2Rq%L*2K81KXT6N$c~bwCUL1RUv48Gv!&DlXX@p76$k989^NOScou6)-w`SI%EboU-Veb4C54VGJoA!P@55dvQOve*=G4tX zyYA?YP?yLiLXWAF?CY&|O|Eu)nFe*mEHL4t@I1(VXg2KvPJepm!JR|X2|k^$qew%0Ut3^e+ACDk{=qh!1b1E0Ssv# zd5@LEZmO>r#K4I--F*zV*U5o z{8d)53MMAieU^__{egq|Z!Q3|AB0jEF%x@NWjzQYH6JU_PsRAYX+kCgZvlcr=Tn3k z)}||<80xPb15%#~K+c=ryxLE#6eTeKv-*rB*Oz>z%k_(W{L!)Kp*e#wTC4dSywD>Q zx?MX%ihnfJ;U0&FOyZ-MrN`LXa{Zi_(f)Xe-Ea;u#8g07T8m?X%arF4OeYVw+IB3D zV>aaK%=Rbl;sFSqXr<1Jli!5Hf)-KDzdfODiY0QK877W})JQ@`le@0ey6$qJUch=H z|4vtzc{{7Z^_^VJ?Or&B~Jb>Qj&ORFZG_`uNNPA45V)YRsoH zHVug}#7?oVji|}4p=J)Hi}K;h4>wD_W@OPfFUUEZzi#v^Zo%%>_DvDx(-G@USVd7s zv`naw-a~KZ{bl3*0q{OwvuLqC{{H{NC|Mucp-^B+cr(YDQfL^W>t6o+!w|u|^@$hZ zBnF80>F`~*vi_qfS}>WD#8c%u+T$On%|u_{>W-!(;)`$RPP>PU10wq0sH9g-0CS?n z5q-GR5TaGCUYw^3N67nh@s4MW6Q}N?dh2+JOnPxKIv7id6_njD^{$hi?-B>+RY9hk7qnW3hdAo?50@Ss^l30gZ^tdb zFpa*oer^0O*oY3tzXLICIhF+wyF8}NT4R9tItpB}r_m+Tllsmo?0$o0R=$h|fWX5TzCCc??V=iVa`(JN7aIhh5bNd27kY0@oEh z>N(J{C>B`sFZyG!@}%sS%G}dC$O#inBa8PV zEblG<^$E=b_7dSG+dQ|FBNGHn$Spp-PqFaYq#P0#yL1Bw(P2S9MQR!78i3!{r9aN$ z_rlO9bUA~T?2fmihBtJp9oz>v{(VfGfQw2oI6^2rLHqbyga2&Z`fj8|X z9ucCt)$g)r3$O%*GHMopiDI1BtTSzl9NE43bM}^>v>lwDh|JIm7?WG{W<@rg?v!5$ z3%vX85~B1Nb&k~UytXp_xmI9*5wHHn#s`MgaSD-FuU>-(8b^|lS+pC5EvTwT8RbAY zidpZL3k-%p4S)aP-`^J)b}{^%d6A7KGxGdSToI5%=T1SY1_aQ5dGO1M#|ZNJ2>|2n zL{F_|r{#iR$?r|(M*z}LhwvT?F{c1x6Ryt9bLOKO;HM&3HF>KKSRo|A<9PeGUlqQyKYL8Ii?SopBpQf8CU3LZ{%c)o7hmhYj=aNW!Afz{TvYr;^~?{WCTPw%#A z+=?-V!|AwT+szMY{TI8b8?kMD`0!=pZ4pc;VUsx>l3v=Zw1?`6@oj~%0*?l4xm+uAR<{@|JiNm5=l^q} zMyopLo|pF@9j~=KQY`OXfw|~A8_j)1CW<(jK)?q8tG&53a=&km1zD?rYO}EYL#a>z zC8-L`AkNVvWV-_bOi{WGht9#1NH#|hz@QK0E#uToa_mWa0u#$ZUe42(Qr@oLO@`-% zg8iH+gvK{DJ(WM|LBOC{9c%;1iDczlnP&lixu~9|=FMisb%Pva!3V|v^D7~mC z;$*oP2;lPI?domy)JJiqa4zdLP#bzTTw~F{{$dT@Dp`j5WzGHTL zYg-@4q|h@GqXBSjR`8@kwj;hc7=<6;gP);~iT>{~do$5dS|i!{>S*rH1=L&$CAZ4q zgEzOf+3jahd*;pugl(Vbu3{2KT^wABQWvL(BIp8R(7^q=GaNddCn|eX)*PooP+ZbB z|L<5Phfq4su0MknO_~Kg2m8wnNnkiy@AhY?v84)aM~HD$R3ivc9QqnW!_&{yta{qx zBj3eYf=B2Ljsm4F@KU=et*N>8b1~=7Al%a2i(m$WQlea2z6BKd`CNkuOWbUNOb`Eab}u@wc_a4jyVmTW3}Uw_lIuqE zQmJuMTyOpW)U?8L7Z!ladXeobCCS3V-iCrhmjnU|finB_Q0Ph_Mvl5*Pzd*0z)e+k z9ZPaK8eWB3X!1qZKm^5n4^oP1bYe}~OM0teLiYN{Yd8T&k`vczXTwx(Lo8+w<%BX4 zAZoXH9NEPSIJVsFNBL+t#^`pQyhdLlVC5oN*5tSI|AI!#7a4RdRu&*`rIziE*szP> zV$~Jy z!ircgn^z9GnOJ4pK;<0CXsQ|!wOAb{0`)@}ozz6lk8Tns9FQKcZlnM98uRoOfSJjm zWVjz@CGpfU6NJ?v`)xM8Uks)Gt&_j8mA5^AeCrK%f&3EX?pxyMctClT@Eb!bPGf?0 zDR39xC8-PU+OzF_093|;cw!GGJgcFaG$(P)#T)OQg_?)mL^8axi%_zj<7Sd~#oQ9KR3`oXC-UO)ul(ZQcTm7h8D6RO+8>nTYbxkk`B5M{lcoO3K3@66JSPo(E2w%(gANi#VD>=ap;ak+yq0@!wHn_C_EZ4aVo`5Y z6A6{2;$8uuRG>mk35##6GxJ*^q>-GwWvqGt1z+PaUPt1G8d_o30_3GTOT$DsV&kKy z5QNQB0Ay)!ip3#sx8??t=tShC5Vq>+L--{U$g)x~W(N-7a759GJpJ;q#+iOnl4hg# zI+!7;M7QphH)`TZkd9a#{JAJs+v-nj{_&$JOmq9_2nD>T;Z;UCG`|H$xLEW7h8DzX z=!J6#&UgMh?HhZuml5RONy9Q-k)qA~l2o5-On6?oPTK}bLCCK~aOf!phGXV%4;`A6 zqL?`4PmLg|S^!j_-K1hur@L~FfD3GtNE&r~-uELTQ`w7+pDgN1g{q=17(Pc&`enuY z{OWd<2?*7!vU2!0u$2b>ow>~pHmI?-YC?6QcTGy;_kFu6jgtdi;dDA)19e->+UqOxmQUq-l zMmEFGGk4KD1mMWP1hgY52c0+|!HhHQ!G#S+0g+`l68U3LGsJC{9H1a-t#}gX(HD(~ zjxhb6c=9}x*A#l;(jVPH|GYyDY&0{zrKKAv1OW0%WK@7Tn@jyy*`s^n-4#qH+bwp^||+XH^I!mcnB&Aa$yD$H?b&rZ(c2d8H; z3hn}sdXNuI=z*psia*-|tM@HP)&6(v#6S7YDR?3X@+e5t{*f&3WQ!YZQC#}nJbsPD z27IKG;ZS&je%?PMY^znCWjZy4lK^)O{jep&~0`*<3zXwbF zFzPXNJjKmf*Z%YvoCfk53m!kvBhLQ+!Zw1V{e;e_Nc}`D`_^_glf263ZxZP0bPQR_ ze&YJ6G7hs-Slc^ziSR@VhWKp~oH+C>GY8N)2#Kz;^vTN#xJZQ{Y-D{H1WwbT>NTxJ zXiHc;oT5cf>Z|0X`7Si*=Bn$dq~rn}4U~o^)kLj!tB|0`MVRqF+@r`=rFR0L(zB$wz%f=FYmj%0b&%JxC!FnAvMY>N=f}CiHC-jdHcqf#B0zWPks@*XSD_d=p*47ElN~M7#DH9r1#hV{*|b!ID&| zqqS8%!2+~(t5Qe66_hZ+0qoX*>QeWT{~Woqd#-TH2?2f8NaNm#5(0wTFPIo#TY zsfA{A_}-%BCq0Vjz+buACj)sD4!dLjR-70@{JOXt3=o-9h%f5J1%25c7?=eHj@|R0 zfe#i-0k;TvgHDOM{YU3w-RYQ07?i>XgirWz0|ByMLLYGtWxK{_TG_XO+hvAZztE4d zv|xDHsXr^tD{u=o1X zz<|FWZ4{KpGht11xm}7<%e?wnXK3#mpx^Qb0Ug|5)rfG;?MK_j;(OnKpke-QlX9w12~W_7qyfu>yEtgpHpjc4Gg zyq}_ynxk(`9U{AX21!d9%8T>Ay{QmyXG^DQSJ%OZ#)lgEnOmdx;SFjLL1IRs@bu6n#WJ1ly8>H-xvg&$Y#%vTLYunvZk-3WM!W$DBD-E^HH&3 z2WSww2J&kd0IxZ^N&Dw{EZ>3Dh(h)pg6D)9OF@qxf!T?DM<(FtTx$@P{|%SAN<(c0 z;{nfv0VZ;!z1O7D@$r6-t0|E;KU%RzvtIytQH8IZ$1_2Hgr48U3jji%D%p8rW`X8x z9 z(i#c3(zUeyEDUCZ+oa&eg==8i4cRCitN|93#haozjvRJD^?mmDY5X^oa+^p9l&G<1!?pnKWl^C}+#H)~zm5_rKX;rrp3 zsBXj^`!o=?7-u%aP#TNX>JIP*qPk6Q{U3Z?aPYL?$C<=W@A;HJI%@QO^-lcvfzXxF zOsGd^0hiG_s0LTY0YnGro#6(%ff76FUhA*4h3v-#(PRw`i?ZkH2T+X*iPuSSDh&;F zZXxKPJN@y{^XEto-6Zw6q0f!Y`{8s0lDOLsyxbel6MOi-lU8MEc-y;uSfJtrgC0@4 zw*c+N!Q%d$+7mr%a!RO$n-3PHzJiKY6`Gdz76E{YIYy;%C96u8C-UMSp3d?X4Oyb~ zoH3H2q*qm;RA!kbj}kee{c_EbGh}eEQMCjF%3Fr0R)#?QEov$VxR(qy^!|;wX4>6~ z?FUfvi&_5@^gjg1I~exr&=6|`Qs{ozH1G?PY^b;ecObvKSKDDN4~~!-IvjTbRlt_s zuvpP*13QFhqS-_N%ls4OF3(r+p>)jq-v2Pj0Xn{(GPT0qE>7_ z+WNjm>lIl#!)0+?2VGKX`F4il@UwepbvxtP2-&Sp4Y=~~0->K;$^5wP6}ValP5`;| zIix~K>_rjiS)VMTMJ_m3=8uhBDlx7nE+lg^Pnr%UoMN@MSFOOTkX00W;b$_l_x%~r zAK--+3ox(Nm!NRsK7lK}!0XUaBa;RPu9~{B#g5q^12hMxJX{2%y`#ejk6f+e1ZL4~ zDHjbePbI+-nu|gSc;HqcaZ`*Ier?at*Ac(f+ zmkk*zH}Dm%-PM~}9b2-i0q!EtZS_6?$33W-)oXiLV}?}V#(MS2!yPC%Co#qUZ14Z0 zp`=w_5ZL%-e34+AbCG{-hV67h0lkLC=S? z4erU%l?wT5cNfaD^Zc$Q27ji;%zaYlK%`&0{yOmQUqQVdsFusS%u$0BeQwGDcvKP)h3Uay=FByn5^2!QLQ-H` zsjxbcZ?uN7$n*m-$nq6M<~VVEEW|SqJc5qc#!&GK%yyLU{tTz%%T4330T6C-!A+ao zJ3nuNX|8~X#yjRc%m{_1BS_05qAeQ9^r6(3R0W`Rs}DrwDJ8vkXDd!U$5nBWJ#ILK zw!~0hvHB#WO?y0jbC};G6p~h&{2)}hp`o`?L2(fw(nQq?J&nRqBi%h}afvo`#NLZq z2RkGD9_vY|2@z3EJZcD1hvEi7zPRvY#nnnF$ON=O@u;^|1+c?nk@hr$6mH6X1ZE*G zbW=ff@NBtsGzl4-Vbdk?0O)=g`15@ja&*XF^5Wlu76v^$N}L~x}!V0KM`21ipm=;O$$;Q>x3vwSR#<_Z%?I{tk20cLLr*stNc z7EuuGPamU70Vp~39j3)zV#1k}0Uj6iACaUaPnz;7;a+C*v0(@uNLH9JP|j@5YkF1* zLfTLSP&VV;=tc*t!g;7CBbsSbKuZ4Vfa!D{qJ!q>-_9AeCe zNYHmd$yaj`cqWbVJBF8TU?Y=%(Kk<5jf%GD_zTIXTN#8J*X@rWGc=p$^vZ;SYVQkv zx{7!~4=q&fY_X=dB5W(Vflrxb??)iJ&@X@NwtbhHuwY)uC=lgXMzB7hs1%k#7PHF# z<|h(obGq0^vl|gAFa2)n74CmO3dT&dbpZz0*&e%6AFK*c4_9<8_0$+%x!en-i9W|^ z>7T~gGZCV;vAb=&t}y${F|R-gy&$^ebn-!N>yOU58>rf!@D!wtRAeSR%(e-GK4;KM zV0FAg`2lbTkAdVIBEVh8Ia^tIFx#iWzWuIKDa2*@&d$ek$Qd+LyjDQvi6+nIp_+)w zbrfg?=a1i5@sutMC<@(tq!f$sFzd$Vp8N#mS~>N+h?N>IMULp!m6$#l{kMZl(-{jj z1GvY2pP#{2*~11i)Sc4?HEa;jtHN)$`C!w@fUe*Cw~g1*M#vtD8PnqV$-!FcBZ1zf zn}poXo2#ldiurT178XZ&-E||kd}|Kw;$mha{eAP#NVx`RrW7JdNe}Dp$dw( zWL43zsQB>YHBdM8E^%Au5EF|0`+e@~ld+}AAWIxBF>5qQaGAZCR{W8OmUd!p8u~&@ z-{Mp5WMq5Mu3J2NmYlp{M3(bX_h_IlOaAji6-i&v2#ubAt!95o5(SYrD+_Z*C7rUe zc~4!PtB!L#bc0C0Z-Ev{j|t((h^Q)EN*dC%&|Gb{nwWIYXbn|UXLa?EHVtLvWL@Ey zeDQ4cKs-WT;fd9pTO|5?`lcEDCDytov!G5?lM?xuUsR-0U0kH9s`5uzU)mr3zg(EU z8WyHqZT8`B_P))4ak1G(OJtvUzn(QlG{L5$;SK984ySCtw>}x@qqAly6@NJ|?8i8+ zWbqsypt8U61LvN!%+?>_!_gn6>@YjIS}avD`#K!aJ5+)h_L7pXc=ZiZQnD*WGV*6H z7LN2`)Y!i3G`YJGQmZu7?T!#+>|ee@a)p}OVRNOmy+6~ixn#q+uDFKSw0MLYpbNma z2c@$DZVi;D^iI{oj7(YMMzh(O`!~!clEqtgr}lSDOrSSuV%sTJe&zBNH4&z_`|g4h zuTUERme1WvyG->Boc)gGO(S3ex6Y1~48n1ckyf%iK5#U~^g~5uK7A7k38iLTV)(U3 zwRV^{QlayzdU_#k&KPg&{xZ`pv3Pz*6=`W{N*UUZ?S^8OmN^`j{XC|fbn=&51t&F9 zoK{se%`#h-oyrfwBiYn!$5}>7zZyU838n6Omj9&%aK9oItQ6KB|Hc@jINMfSV=Ll?fyn~dCea)ZXIq6l zw0dUSOKaATUbPwbY(uArM!EF^Ui~+H*(6R47oW?Ra~lU=cHPP-bRW1+wK!H=9loys z&PE@Y5elrij%Xf-j02{dZrl?KaLbr#&t_YE_G6RwJxffX_}h%cX#I(p1kSbPC&%_X zmhm|Tn65YH%ecFfnseb?Z z#LYh;0^@QEDPlUV9Hw$^=;vyet4mW)^lmb2<|z^tVKP0XUZP`~B4d)LD$!cDcgwfwZXL+d3N#8qxOBb#4Vm=Uty zZ#AYhv$)*SBI{A?TBoC{;^iHIIErb$>pcJe!fr+L3c`U#~b?`{hG(mFgEJ>>icIqY}~mg4u1U#;yIVPHgTj?WE@hs z&jWKJ*)}CthCUY;v7bw!e}wzDx}}k9uI4t zMlHZGjF;QC&ruW98ojBTQ+UwmE;o?7>56HqcIw-6C$sSBpLLg1tC`L0+jm;cDe7XD zX{|eKA9UAtTKRkI4zW;&q$9w>5i?euZRsQ1-4a3>(iUB5q8QSlq4rt*y}IR~(u%;P z4zdC^3%ES@Yqkre$!+t&f@A|j!^qt={hB*>?wF5O6#iEA7I1s=NmV(yJu;G?iZo4D zh6ko3%BfwkDX$O{QzgIhIq>7P$mZ{vC1#Hq9aeL~c}FMvrW7?czAoU73vc8{?q|I; zIDT`nJi4NLKPxgnkcp;ZYtNr$^ zq5$tnj#Z1w-xyoJ-rk}j$u=IV4HmbYlDnk|-6}n@#rkGJE2(KwpFUOF;{Ni`Z}s?& zFL^cG$hdut5DVAX8|&QaCHL|C==|9Px4F>{x^wRLH-$3#if^iDl{b~<>4pP6TJd77 zw!-}Oq^lo6#ZUqLuUD^3S0=R=k)&!I8xgaC6kp>_$x<8rdG1h*Zvoe={q)M6JYm{X zVObMQ)Ao{pJFP_|4i5F|=Jb!|{P;IE_Z=NDJo2&TJc-v&z3g_}Lor6>=wD3@4FB|^ zJ=;dspFzCnxN3P%Z=o~UU1NZhF;V<m-&8gZU#Ll*DJlX!GkoK z@Ni#G>r%^JyabB2fm|!?-9#V|{C3UeSL>VeT^ipK>~1*kxa*FT*q!cEaM@PaxR;k% zCHIi%c)r+imtX4ZvBYrXYLgwDGMID>e9;r~JM1)AnB<3%Rw zmyTRMgyrWeikg~Q{r-)mXk*ix_vFfh2M;SN%WB}|<1;B*ZlgRA za=JwB;wip7wGvX+7JL2I;=xyG7Q3TgAHEE%^c)l#+Z zcEF$Us#=WYkemDX4tb3Kyk~OTp*MpeKW~Ma#CPMm6u`A|SY0Hp%dPjWYtItwayrw-%U0E_$dS4Ximlw~<3A$db z3%-Umy@xd*1Ddp^Ib%YwOQq_qS+h zDvIq4d+iDmD@ZrTx+)4AkK1$SM!eu3;!o>5!}tV|6~e!)-h>VR@%v<9_4Y)|&kuIr zOiRcK=R2_)oX32lLMW2;aLm$ziD9`%RF|G&`tSH> zYKN=!N~;lmJyoG&Gsw_d{R{fnbl zeWWd@6JLH!Bb*9pjV@fvuj7)HQ1#Tk?{aw~(aZYY>daD*@KkfLlAMIZ3_(z-goK2u zc};E2`q1>x`SHT*$Y2dM^Wk9CT2E%N$wrI7TESB}4z@3z9$d@)ZN~19h2ADfrWGJ2 z`4y268u@`~=-Aol8ux`)l9p2sBqXK@G;UmMygi-kNuxm5WWe;Ke z6#V>$OYQXY^*-{t@eG$^#=88^YQJz?S;@(huYKX0Z4nU1Y`tTtbKj-gxW;7yV|tY& z^#KDsuDf|Rn24BI`IGCxIsh_FrVcm20S#LAUT2IzAtFL5d=KB5tbf{(liEh8!6DTBdlniWK! z%*>2v{WGWvK0eaC?{fF9%44f`N3Y*!&kW>dKVXILEo%Jl0ar;af5w-T7gz1f_GRnn z%Beo0#O$9VBcv`Ox4A~*OL}}@_TP&aYHBMm2X@O<`g)~LcVCq}|B>H9eK#l|Kue0A zfvo8@du0%7sm15QhNt)1`${DwonN$)E^E~c8VeR^aZA+w4LlbB3(!Jy(al;x`D$$n zvmK(2Z*P_JU+m+~dcJakb~#OUZXH56Y!N3-nmx8s@B47-2Us^EEK_+iNh_bKg9+2t<@2l z6;4x-i6sqG*k!0z7X4f4lK-(EeE8!?wJaTSt8@58brMg4@rcoUP8qi<9U8VB?(uK53u1a_P@z09VZ zJm0RFdVc-e6^a{hKwsb=eC13IUf(0V^e?@^0X z(R-&t)AH+AT-~$Aw``&9R=2OkXx)k;Kw{3*Gx!kwL{5msa{L1V_6Nf6^E)nHonKk; z$R|oU@BJ3J`z2z|mx!{aw$@NRSIhD9HO^{Lu#yl5eqAXRBR6^pJnwrdEou)xD@Urx zO3e|8Ibe5Q74<*{kR!HKoo8GWg#6djiNvm5dh4mu(yr3tim0SURNo?Pt=Ze#yJ0f= z#$Wp4(AklV$LR*z{b^kAUq4;B$-u`fJ>{dS>~gS|)ObZv^mzJ%&Q!Yda{U6efKs)U zcF23;%21zLx5E8qTB{~DwY=c!<}klbJ9*edHJw}iD#dsVEywbdf4;pxY;ti|cqfuOtFxpe2#qsNFKo6nDZ>syB`DlK6ol243z zV)aAA!bFU12}_0T`HwzweFvvpU;D(w7*3v zS1h>M*w{ElEvdZ9xjHH#k>`z`|F*-ehP(;S#=d(dFG5a*ObE_(B(&jw$Phy(}727`r!)N{a|`<2m)A9Lq#dVBkT2 z6`lnuG)6mewUOFe9{^v|cVBn%jp=Li)z|l*C`)=%ZKCAaN#3S&Y2yuA! zQ@JxQB>4#m9Xt=rVmH2hmfEm+6bvp>$0tJbo>k1TaOoS|&k6$76FXO6rc1*>3Flpc1vGi^w_J8-*jFX z=6;mkllv`Kj0K_kpz@GBq^U(czxJLrnQFE)OSHpMf9^wNv7P0idlGmXSG<>3G7O7~ z`Zd%_!@nDx$604XXpym?yS&8VY=6NeP4kY6Rtv2CA~$S^D(O$s2He^I%h-K$ccL-w zQziuL6|2J;vbf>0HshLytx@xPy^sI%5<;+0b~?RF+th{z22R?9O-)Ungp*y%hAcDF z)oYlMjsuEXy;COd<*!Rl^b!J}yDA>Yn*DSQW#r*;svWYlzDQSXV3ob|4#sE~X3*^bWJCg?6FEXC%|->(schEEC@qq}E@OstynbSu1T<)Y2sy_MEJ zf4K8(lTc(Z_^?0`uz)hhQJ}OWlh2;*%5W*!a{ObpZD(Ytj)SljPKZmaM>K~nE7p%E zR&1$MS12iC4Qvunx5P37L7d9?_O;hJp-Wo232TbK`QxptBK~s6E9`pfNAlRGD@2|eIH3Kp%sXw0|&|2sFFfqJvgYcT22`=7$ez42feWb=kTCwnu zok+PdLR~a*c-*k5o|8*8_9>u84*&03tXaKF1Nj{s^VlpiWa?q|>cy@uE{=`4<>NT@ z>YuwbpZ4&;W>S^AR$_aXt0n5-(Z5JwON}EYA=!QeMr<>F-@w42j54HY0jLYF9PRQ| z%LlvmJ`pe6Jm>!W`O}Ui-Nt|E7i%0ga&&wwfVJMiKboDj7+BL$9<^OK^wDwtQy)c& zQb0gJJo0?4rGT4*!&=UBAWy$e_yOKV(DlvB#FZBz6!tlI&VGsuQ7lf?tqWbbNNO74 zpAjq*&`0a;O&JitlRk`3;lsZ&VnGz>@BhQGP*YCQ(Q>5BD)X%F5H`K>kSI0kEY{4eX1l7-Enru67$bLK0f9I=gPp!x+wXkL8&!-Kd9yT zU6LpPmZds|`}c>M&1*vX45joBv9gn(W_ zi}-V*jzwiZ70Y}i`g4`a{_CQh0-N_Xj3w_F4sNZYTfj*QBl>fXWrjskxr{mRZu`^Q z)8;Zt*kl9wa9t9gVuXr%sjGW;!g3j^^+838_gS%?Zp_#@gfxip zJi$8^tzF0M0%0;rN;e6-uJiRT5p68#>>g}UH{|h`2;8fZE)C&_Bn+P`6nhsOuBx1TijT*dvQOrlHSvg52DufYmNxs!RTzCV98J4!6Dt>@Z<#VS*|5H|+riD0 z`t5CgT|YCiGRynR7JnBWk2p<=_8KK>0pxct)LfcXRSUr?_-SrR>pda2iPwEEb#ZMz-HZNiz*6v(92T+3l&k$NMTpS0s|Pot*qCL@o=OFv>S!}D zc}ug-2?=$-Wbcu4(QReuOT_(MyY)5Jui-B7^Vgr}D)pa#CAC^hHtsS`3b5T008g3o zsP$6YYxYQP8!xXCw=Lz^jSt#olgkN0;JsOepbGa*#?8ZAyX5J=7Zxkf)E0KYN#Hr)fm$;X_waF2y>};~Ff7AHq>8t7nim z4M`eTzb7%b_yfpyUKWB7RT{0Pb_|VULIm~ixpjUT6!x;sC!~HiD6}l&w=}tZ&HZof zHBs;u=@CUA`Z^VG*RI$&4l*qne&m$m<*+AJQ!7U@`UO}QgVuykuEFF`*4B|L%4cU+ zW_)*>Lm*Uf^(~$p{K>H*GVj$-rOx zemAhXpOlrQs@9j0waB2R#_lWfS;GC^y?gpCYhN0k;+=ihL#qzwtc91G+wrEgjjLMo z#Kh^)Yiozi*)}pjDv_rz!7an+H?_V?lQ)rKAEGOOJaUg_0pJedXUlg1-ZCRC+9aW+ zts@lOdHbv{HR9|}Cq5qD%X6YTi&H)jZ?*}>=dN0pQ)DE>#2h3ZRtdNoAy1aB5(pCHTa*KI~Q(GOpL)YmOKFSloL|?^D35=-}X>f#4SrbmZ&Tok;xV?AZSjK}!Ek zN?qKB;?i4%ojmSqm%>$A0*0E!{5w)32*`e?M)c$mOaz98mOT-og!F*T=cBLov3r0& z>m^c484G#09f{J}HmfFHk5i%UJpBPkRz5u6&Xg}4NZ?&Kk;9nZ^HgD&OTX1^v6h`BmVqT#JbcP?}N%oE^`YYWAoyn zqkO%5n_H>NjHy&%Ibtah-KJa=Cexoxc&xX-;GbC+4Wkh+(N zM$ZBV_8>B;3%{L5DmDt<--30qRz*dH2DdvxgMgBz`5@Iv}@&ZmfxImwV zQxP?%!|#1x%zp>K#K>kW9S;vr*T8@Vu~;y+Eo)+HnH)=ENKnwyza(^r<2FS|D{M0* zQa9OuLD|g-=NcUNp6;_wtAhp48Y91b`<5W)AC4$Y+YfQD%xpgVTxbI&*p^!FMDdk}d2?`KbtVlY zh%<^6qQ_a%S0&BxN3eT=iVe|5~TzHsPA8JI4|aZ9e}_6;`^8%KcqXny}d~RI0q=C-@8b^ z)Aa4z(b6xst4|F6JViFS+e9TI7=^B1*N{7dWIQfr(q>S~94UT&{*Q4-8TlHdelZeK z7VrEs{J(rFDxYWyXp6N{D0P`8cJNrek1qyaTj`61hp%g}b_TX8<0!CH;ycQbRbGQL zf3e%3fwMy-to5eCG{x@4^9@rIo)f^OuRHyTp~iw^Xv%mRI^1~aA0;=gLk2mdVbY=>qft+Z)x(k zQ`6^yKglzF^E$bIpqP;J>@I`Amv6&_!lEsyay;o;B3ndIe#v>j40z%18!MvGxD;lZ zz?5%Tn)r02^)!;od%Ln6GRjwRyrit81MV{9s_q*)=tak9kRcbxzZ?% z64MF|`Dv^9rSXOBNEH9lxx&Ii7UV6zg>Y7<))QJC1=+8a7s+%miFCwjkA%m^KkhAN zSNuP9y>(cW-xD^zv`8)pNC*NSN=l?#6a-N~kZ$P?=}u*76%c6w5u_U=7fF$nUK)g@ zyYoE@`u+X>cwaBPmP?;K&xtuR_uMmc&g=bqDs~fnB7|f?;PR$NxvKplZyy7=`fOOk z?_VxJ-?`*vlL0C7A1Wn9Ie3puII*w^OrQx?I#K6I#?GQpI-Pw`F+xmCTX;pr?u4{z zADY{LCyIx>&nrEih*hzA9|Y@o)g&c_;+aLT)gb#d&Qd8Ia=_)~9672y-t8~f;6BnQ z$W?xjyToC}tFD+B9NQwD?5e5W%(v5Cq=-bPDj}L|Vt?`nV z_>dgEOl(@OttJxl7?qmm_S3etZpOCWVvx@|A%siA=|OFwVPEA7v@Q4zA+B87vDRm> zei-lJM38pDx(gfty%04rkPlZ$dH7A2(r3yu+{Y>u21|}$w+0`u33%c8X^Dy&_<8>+ zR&~w8ZlW%Mw|6l`^jYjODA08Wmkhp(P1eouNM3G9;1w>~>qjbaEYCUNB{vl*5Eybgnf*);vmx-H&JKl!Xs%eMqK6=pN8wRG~=SgqD zE0SIk)VRrx2Vn5uK()wI+yo*JfSjcU@dt()YmEZq*kocDE&okCeRnjo7&dcFZ4Ouh z4+e&LcVJKR<7R)=RNLcY^W@RGF|#`P6p^K4g!L5-k}FhQdNslhEG!Ux=%9aW(?%M7 zr#T7>4{SBECZIe^n5;=3S?f16ED63s7h_lZeI+ohO>wAQA#KXK&_<9f;zgd_lllge zn4Bb#Tp16+O)BvB7fFkgypMMbBltM)F}mzfUr%6yD_3~X2z(O2y!TPY@+x| zVEi+Meuqb$=Y=;I-JJ6y^i=O-QwY2-0ApM3^1|Km%spAiM+AugA*k(b(M@oDJh8vd zo8cixN6m5@%BV#)MYG+#y%zYz`Su`V^q*UA3&3rP=xN}1sUDw@8V`Gg6#eKW4TIJq z?)uoghju+((p?dh?iOwMchdp|m6c7tX1zKN#+6fgS|Us#?0$d6Z8_sgeT0SB5~Ji< zek{82BJ5S>lQGUg^eXkQg#uLD6V^qqbR=m3Dqc^**~0W zzL!~|xf?%Nu2qH#N8RNVzatgLj#0hOb2J@7XS4ze`E_!-8|*gbTHQfH=6lawGs`Eg zl(PznpNIU>1m=+ko9$DQmxFhqak4xE+2i>$*#08xi1hisbko!jF#8#Eoxt}5;W|FQ zpCsO=zqp0lCqp|3+@(vxJu6XqDr09yV2oQsQy4ocOn|(-tp0&hzgtEm`z#* z_-l)`4rr;K;u0Q*dVKa1w;}IR-=jIH+mFRk3l?JifN<|CmLkQyBoK_k`>yb z(nQ{Xng>zuG}F~#dfvQfn`p%pAch6v;NULMi6_eXfdgX4Tg}36KwaFyPN4m8;Z7|P zO~=@+e5YDhFohobkx@M!$=>VQY*{V}72}mMWwo1?l#<^~@h&1mK^W?LEzq<1?uUAs zr5EQ~FZDff)u;squXSA;h}Ml8ncEm7WZ!-Lys%eeT0V}rr^=Q@S-8>z*^}I^L`17Q zQ<2`(G~khh)zpk3Hl0!R*(xwF>r7lL*zUBXW9-dBW;ng~3W*SeRW7gyCNnE?(%H520$rra(w z2k3j^eYs}w>)-$VXL{b)tiAt{v;Q2NuYT2Oh}ZB(3OzJwj2k#x;kmh$o5l>U(Q@Lf!&dvr5b7~uoT$~Fh#i*$&Gs@0^ z@U2|ci5LcCKSt^PpCtR4xFMoXtVMv#H?kjo>pNWM?c=yvyZNI%;4&JOhp_zDYTZ)Z z+5YKaqxXVS?t|L<2k@plqvz*muW1c_y{s;i1?&)|mKOBUEzfRQ1MdK`l9KOjR_b|g zQ7FWBvc}>?6<4C5T?9l@a?f+G|E59yJ_rMk%r-;8jVOiN05IBo;>GEF#%&oJp*!|% zeH?0@t@pNXxer-TMXW1FO$PSwJ)gf?EATTMp4=WE^TALcX;xV-JTN?-YeB5gX#XrQ zShGO8{zEKg%$Tkrd+?V(Ch^v6#bV6N*)Q@2mbWI}(J#4;=;hdC8~QA)4C%I-#f;(i z-aPrZzUXr@^Wr&v&l)1m+Izax6odlauM@Mze|edWk}|pQ)U)z^Ugc%;aW24pMsEX_% zoNhls9Q&Oq&s2aS1ZVfDKz9$11u-tmXMq?YgbQLDeKjrtBNtA8v9r+bhmjC(q za3W?`$n4y6O~T>sv~{CFHL*saJ~$ec@W#XJO4~aPN(Mpn+51h1>T2CqC2O2$8mYBu zKT$Dzsr+$nU8iCed*6J)C!#OmozuW<8Ur)`z3un!foIN}J2NPqRIiv~)23uQ{#NhJ z*$4klNteav|E-Oi^Xg~0+U|L*axUGp?0tj+jNz@W7GEGaNq&)UN zNxo>-lN}owF)%+*i!#VpZ;7JhLDx@BC7jMy>`rudhWjUq@)PhnM8#y;rXdZC8F8v}>p?xQyQ2C2CcW1w+0ijAAlX>{umW)ncm95x)%)}5bU zD=(~XpK5tf;5+Vpe3}nA1-C(4mn41``pB?EI5OpOaX!4uh z1clb7RaKmE2JM12@VEAj6_g{(`%9+><2g3__MgBG(#6q0C4fNV7=Sf-PAy6M7>6(z zSY`-Ff+6Wi>s8CK)oqk8tJ@_6?w>))#{9RD`St!xr%4tt+aUAr_00egE<8_hUGF<-!m(|OQB3)9!rcg9rRBmz0EXuN#i^?oIXicK za3UzlL4jU|*TL`XDwq8;SPRhBx_Ik!)P@4xGFg$gWWMA1Q$9@2%_T3`C0>wKzSY=( z-CX`E2)q6^IaRI;4MWC*IL}??%FVGLPzP~$c|{GxK|eon7ebWVguaDBu_O+GS6Q71 z4(_xdh6X?+8@-F_-zSD0btgH_by4cp`ra98ZtiD#@W8xvH8lw5Rf*=zCQ@U`2hWLz z2YN{>9u^+YooVH8ADHCeTzY!BXG*9f&h^War~ z$9p+3&%wnBaZ={b`te*;UouLak6Um#XvL|PwetE4P|R2@^>38UO!%8LSPY^+w`8^< z#W&%4T8~fBCVqkh90nO>PlzVw-RCy~C{n#iFisCgM96_yFgBiT<1`#Cvq0cZkKfMl zh_WmEs{JyMU1HdB zR(1viP%-x_#vv;D*N>65Z3BgR&51-3#izsWul)pz^jm+I7_XCCqVs-Ch6kT+lc`qUvuTY2LP%*KEU=eJRiC_=`P&GR674I=fh(QrE&Q?*%?6jy`{+GhEPYA*m@!6{REnxJ_ADElX z6}e3{W#!ZM51-|!xB7s*sx*HH!3WeDIbNp)LcdpRl5@ZZdnJHh23^cQ5Rpgrka?PN zI^@aP7NN4Y?0u?0MjzPW4+ee%_}#<6Ne*H!l;&=mmBL7{Ca2koZzKhs0fqCLU03Uh zps-`#U+e~|!CB;vr^{y4h`zg2vua_cdXj*}=~nY-iG_0EcAk09-HYAc{^jLtN29}y zWJynMZs~HXH?ZLXsgWmG0U z+Goih?S{}f9GrP*a*z)e=}-VwuigHd?M?_m^Ky!)Q@z5JQHfz|yun;M+e@3m>33?Q z`+ckZn~j&P7$D{!ME5ejY&OZ>!Eix7$q_ij4fc(ZTg(dA$e5}1n$Y?)eCF-b51ctZ z+tGJ#v_-`Lya9fedC<32Hnvf>t-D+c%ce>3VmqvPV4M~9KvNbLK7BH1Wu zY(b`hK}qZ3yFl$fL|kw_0XcE%;3lkVWyXAM&|&xdHw}EqmIdv}Pmht&w5>&(^~e@! z)mx|W-0g^HiNNxgIkx>pbXSr2t9~#4uipoG_gwQemFGN$yApzS)v4o@$3|KbJ51t;|rg_ znnmVmqzHfm^W&2RD8{OmPd7_!r^ITJAOMhw{KRA916o#9+-4U8I|r&PyqG?cX*Tdx zF{w!L*GI*qa2wlCye!j?Y=I~Eq*rjtFdZ;qIW+Sc9*0#!J);*7l%)QA(oHLh^Y$hN z20@J+kP|l4SFaE0b-%l9Sn@Snk-D0z#$LeK5z zb;P$IvGE4jnmMTW=%HplC!Gk|5-1>sbgQY=7dp~1)T*jY-uV3X_xD{v1HU8sw~_~+ zhTntfycb;QbDE)IXc&sbEdb=1h@0yphR%zd#QPFFfOMkSM1er4q&^Gv&Gn36y#NKyvkdKDr5E8ZW)#@4`fR_27zBe1w#nBqCTsRg6 z^K%Eg>YFBqbFG?}LmKbM!@Gr3LU26K4jV1N)KmQje{Y7~rV1 zp53?lrstp?i;d}HPio}!ly5B`!$)iK7~B$GbB2p>?lp2NA9Qqw>A`Q;y)pc^H}5{W zv3Jqyw^f*%akw>WG1}l$_@f}YQ8LkWJbei3)V&ws^_iyHv=E4H(oodY7Zlq{%jYW(<1wUfQ5z$UnFvSR}A2ckUYcM&i( zKsrRt-&v7yP=DlpR^;WTE8-%zd;kL(tlT|jEI{RFGpG4$6;C}Sak%mZo9>HiX{u!;f9NL`h$_z$b0QjgSFl9jpE zqm@vWcc~v#)Ey@~LD~1#>Mw4jPL3_$<4=?lTTnqNDB52g0J}LmzZvZ|KflX4YNuaC zXdrgN3ewCzONor2sBafKpWsI3=MSG)<#v0d=o0}R=<_Ka_vkvob0Fztq>b#LU;u2P zN2U}uc{0+Y$uTNMBa7WfRvpB;N655NBBYU`-B_Rp2tUoN47v86Vq(u-e0$lSVbJ(M z5hQX-7e=)rUO!Tft-U3)T*w;MCxWLe1z^gl;lY|6ASo^G0EvZ#b+k{@k*bnaPzhEF zvt+eg)O&4r4=5S8XUO=+QvElm4q&mSppVFf_bTUv-1BlT1&7I++ecu5Jdi1-$x)z* z3AFGEROUI647li86Ynk|-1T2HE0z`##Tgyy;W)F~YwRddOAk^m->>d$?>6h?X=K{{ z`Y3?>5-a5W=Z3oX9u&3ZzL71(m!ZO06Ufx&Cbhhs`mccfb3ckvBG#<;q+~`3gximd z)YDbm8Lqr}0mw!_E1vSJCulb?AmOcn9?w3)O$4lIP|=RC&h}oS`H!5B>xT<$d!Dj6 z{yB173y*9)yF9~T5wIV=`)cf`mcr>`(sAL}{hi%ry`uWJUd)QY@kREOEU3h2w;w3z zb52OPq_W5cqX9|f{M(&m$@5SC1Cwj_6SQv6p%wV{{d)l)`{^x6; z&z*R5l9F`?t3{tAEQkq_T2e`R#kQJ7v6X2z{tlb=8r&DB+?72^ zA@kINi(ZlOX>>IJ`6o{Mxc!}__Ny`kD3mNII*m?WuNcCMP4FWf{bnVk_@J4n*&?f*c_z1~tR!3wP;P8q0?QJ0vy6De|{3y07 zk^mbsuHQ<>vSg;KP)~@8E;@5{WZvxc>-1jSTrE=6pkrz`0u_7d^1v*02p{QTfrdfT zYUxjbq%~Yl!i^9~MB8$Vf%7ggR`tsWDfkvc z2)@)gmmY*ZY)==ZYGnUSY*t2b;Ce|6OaEDv=uC&FR`^fnygn=QvLg97_Q#v^7?vdu z<#FY3*_eL6R59%7VC}cg0r@KobYA$NjDoxWs>4zLw4l6#6qZie&)^`CRSRG0zt6-x z*VU6!6;>eQ%qvXkP#hV+xHmNg2r4M&>7i_adO@-PH<&}^SAX$WhKkyYrs3Fx&}qqw z+l-CC4{VQUf~vJP^6&_+6Lp4VM-wrKxJn1niWB#z2faLWVnuV2kIXXX1f^kJ+|3 z_IKpZ8V>h%b{`wM3!&$#U47(f$hyZc=OA*RVtZ$fKxSaNc#Wd;sL9WG@Dgl^3jZw! zK3pQ%n58A%`QJuTpje56nKW1thaU4TW-xJ|msj(z-l9PRphuza%a+OsA2L<`Kd=|W zZ#mbdO2_u7EA>=6lJ982l|InK{U0XU>@0|NgekNe)p!&NL*oxRmC-s+rq+<5zstv7D zA?Zt>sSQO!tXNY-{dV_3l1SNZXm9bJ01**}c3+dO}2x`S`&>n6syovWcLUTJ<`zH14ldnGnLW6iJEv5PZ z)eRLMNP4>KG-dB~d{z3(w;$@G7t=459N(aJx!R4t=;Zw(NU{T8FuIOV!*nt6D#2S+ z`~L|&SJ2hCay$OY9YZ{*qd~Qx`mcpwYz2Yq+bN>klS}<_-(R;<@}P#6{7=+gKh07u z))V#;hyZ)tl-FE@of)-C1OId$*DsK$`8Y`s+;>XnCvEk*bl~-ssA7ayECgq;PLjzi ze{mtq_`w>cV%er);38k+skX|BS{deKwc>i`m0KTrQb66vWOcx&%?2k=b=2^Ie>mLD z7B-j8+Gz!Ggb3UVE7eoJmF~jD;04S+;mqjop*#a$iz5dRg?)2{c{>z@REzbh3gN<) z+&Uju-S;&NnquG24km*)Ni(4et-`VUeyvn<`ZrLS0tsM;;y4wCgAlsck>E6W+*QBhEY(ufn8jM>Qc5;VQv*AAzDpQz>or6K z<584%qBu>voNBCb3`fUmhqX`LM`2oJcR4?*{FDd(qY5S)r229ajbaB}8OB(Za2oLM zf7-&di@U2mNF}4G&D{#|+M`~z?<{0}i~|dh!}@Qy_tZ z0o_qn{zgXFQD|7IfpfyW_Vz*1tZ!-TRgxixSmR2mQaL9>14wcsjz8HX*|L zW&ih%k|bxOJLrbA%#XiT^?OC&&B`c5nJZ@xjo4BPiaVB;`Db#!i<{rF8LLGl3Y1@$ z69N|pPr}!CX$y`)Tr+q6smx>BNLScv_~$q942k^NAsmsC11FU znN%Iz)fp25h5;3h`m5Dm>rh%+-oH5Gy~t)At#P*e$xMpA5-mB`Hk8+cD+{`!D(zW- zfudc(TF5*B0V|uaz)9dD`(AhhcR|$Li0yr9Vi*vk$Ct{P$-GyS1pVJ6%>t+V2wbs5 z14>oDc6F)L_9hlJJW7Ky&MdF%SskKqY3bj{IIM${1{U4kNGWx&S(QTZv+`XF9wSF~GwaOx=7zBF z`pT_G78<}}@-`2G2J#8?u&Mgqn| z$h3H=M__q!m;Tx8n_2ov0`A-6_B<{F%(qiLZ>H+&zmaG^+U>1=AFGPJ43;8?!T7H# zzV~IzbAM}4mhRO_q27|D)h%y!jh;iq-k@qeAl4r|M+r14PZWFr$xg?5`*tpUd!EPm z&`#`R%Iij!DHormwfEiE1SZ zmo00UGE7=Ib{OKiNI6xIYh}&3W1#^2CkQ)auagRac2`AB?eHW#>n8nb{7pPG)q!Q_ z+rl=tvG>;VTsT1cs6QE{e8BEKTT3yd#yq5m90!3n3q1cJ?$>HXkQ`LW2G;IJ4v^<} zae1&IjwsBP*gFy=y8G}C)4hy${&5|TgYWL-%n&IvG6eocfHJ8=Te8*+in;G zKHXBikxZVy(szHm;<#si zAIo~!Y_S_oMk-G9_w`gP$fjHP!|Ltm6c{h_-Uj{ctP3ahg`fl1^QOd#0@x>luH+>T zIjW>dWcRqm1+ABjK>zM_v)a>z4I4G5MUC5SJ?O@M9bF44wl78g*AeQlZ`#;W*VFYu zfc5`A0sc+ee>;b0KvfntW|6M`BuXMzNa!Rt_dDU z>E2961_?A8HOU5gUhb5c<1XxuF3IPA#7;tC1^zg^yG@~~;a8JF9R~YEl#I4^)7u$d ze4cm^x9Y7j-<4SL`#(}>l5`7>|1TC#F7ZmN#X-p=g+I~^a~3m5I3cNUv@gS41w0Zy zOe#)sHBN#1_L-PgXue{GUC-s8tYg97tQxD#|LoGXBngEr!<7QEF{+fdoSCy=M~!d; z#XVG4cmp&P_QbCGfq1w+UVe&g&AY2QGq}&2vTa_ho~ZlvC3;0nzpyh#N8=~BhnO*X zulR?bVuDvM{F)CNy)|0ktNS70QaoTl%N&2W@G+DpPaxiWaR(ttDnvYr>wdfXX06ysiGcyF}pY$2Ybk38AnQR;^kch#=bKFn1d{ z)DN?(Xx^9LjFoqja>2#sdFaAAde_%jw==EHi{X4dSq3AWnSc}IJd}r%Aqs;WEgtO> z)B6pH$+?eoopo%x519k?!doG_BMt|02LcB#X&MS>(%u?xn$VwHNWonR z(~fElf%%IOi$$($L5u8Ph&_5K;Dfi+mNhlFM>oB9sgP#OnLoY}3Dbg>7)vMPOgwt} z;#5|CVxt|pJ=miNEzywH#h#dms3oozW{9E@IEIbhW4K5p`E`oB zMMjhtbn93GIeN?8Zeah69_eX0a54POk?IZXlfv<2uSJcyOiQ@+=~P!;M${zjXxV{5 zLPBFC%p9W#lKN0zrq62<<)yO0%i-QMQ=cR3ob3bFPr;{?RQ{{+jUsN$iVcJIKKT9B*0c zTOqb`Z>CqX_ziF3YFO#Ynb^$z8J}3yO^g70*%o?*n!_>YpbESt(tIq!uQ~{bVG(SU znRn9)N2VphQ^McCY*9(}g3JQbDPVJd&vtxI4@a!5k_f9ocRXa|`HG+Iffw(>)87Lk zcc9xGGmNnh&+pPj!HOW8on6xhW4>aOIE36;-eVC@yLT=`XKr^s4)gu^q$T9RZjAW3 zIl9uuvS@*&J<&vevbAg{|0P?{G=mTi0~hvGVnX9e*?A-!w?dr4ozp?3?EH3T9koT~@m=W1{8v6k|1TD>4i5$8cu!*5FRQN04P=#cMHVn4T%JV@` zr86ZJTddb`{iKAS=>@MOuf4^IeST9(xPOq8zF2_bR`K>Qcr+tXS-CFmh$lo_JS0|!% z6(c!{gq^by-4M}AV`t6cBY(LOC+xiXy$)OGF2myfunQ#%9oE#OcSA*wsj+4j6c^*BkbPbUtgF*`e#a*Z20HmcmP z#l;M&n7_+g^sCD92w#}6=*l7Gl{h6rYWmj@cLT`5$@eM>95*-xKRd!-2%TztNgJFj z&xUO7+sS>!f;bzm2dAs%XR_1mkp_nB?7nr9QN6 zf2?qNkZQ})=kxf)dq&*HT(>U%!z`Y|C!qX#DIZr>6dBm}m)X+EpX0H*$Xr?m@19A@ z-?dbnv^S8y^MW;t805-e!Tp?FbCu{s1p5=q$M7Z8*md=y-| ztzP7co%O`F9S0lJ5G}R8T=HW=eZZ*iyf4_kj5DKVb3%ZRV?=x}EV3gOjhS@FBG)~RCdjOM@ug7QA3QKAIKxiLjiyIv3reQY4XJKrX z+O`tzKu}g~*{h$YweNqP;A$9c&Vd!|CqT~Xr>Rv69q4MpV=s}PD{L8zsB7?Qff;zyy(npk@RDsw z!RQ%6e7*1<1**&OQhTjdVW7cyCTFNZP|6;%A{uMBj^CLfH=TdZog>+3;HN*a2Bt`q ziFhNEBjRhQ%WmSvZRj}0F017!PO)b~pv(580}s>q{&wtZx@?TJZ$#KVSPhfA3j!Me zdvx;mVjN_0P!03^<|o1rdxlcVUtAa&T)!kmxIb=`&aLS?nqO^Swq|LZ=3Bob`&~)2a5Ak6@Zol!rH?5FHy*F{#=*0? zx5C+)5Z|FCq+1+@F*V&JH)5KL5k(N`ma+hDW%TlMF??M`51E;x?Ic8|>7oL(L~@H< zex}-oggxgEi1?FzfIe*^_%7Y^mK4dLXrLNNOycbK6YT;GLP{XyOwA!W_R8b!u zZY%G0p2d`KJe?^vQcM!ryg1mvbh+Q9EAU4sdJf$ri>4Om<)+KE>VjT$qJn zQJL2~AkT~3c!l$ggS0JoHs*J}pE4~2ta!g%&?|`0B+(_%>c~e)N5yyzAl|kh-cu3KFNOD>2rsoKTj#`K=FuXwmy8(9eQd7`{jlban!yPgjs}k!jWu|}M!anC4T|{?_y@!V`)8-Rp_?q`i{j4qbD7K7=Ntj|v7}p^YXaQ;tMG9L z9|^*77iQt^J*H;TVv-C~w9w5>L1x}~vqcL35#`k9QfxoO-6FER^amtf$$chQZ(P)> zsN6(o^6wB|A$C40{`&|mC*()8RhIReHu~cuCW||%>zQns_dhx^V)wb$ff#C*w>h4IW!8YxdBhAKtD zD52ZIU;Vz!EL(K4Mwm{>Ay4luC>XHvggIG@oF+2I?!(hLuOMuFWK4O)5!O@P~Jm#P07%mey{6g6d2^^3y&) z%Ii9_ZHyTdv=t5i%$@z8G~ok2O`T~~zdMsAbx{wkGvO=}kB5x11OaXm#wo@ed~6#3t3oB#ahSa>jnZJ?vLtslxnG zQ*~oBL52M&3|4(Z^J9X~FC6?23G9q>=raoytIwd0_zrevTzPr)w9;&Ew&z65n3#%d zxk?{N64a&coc!pAI^u3A&zQ&BjI9U6qp|RvdjH4k^$f2Zt-n{M1&__eTJdL>C|h{A zOR2Ra3G`P0?}Ob7*ytw}?nT>JEO6RTJY}1p=4l-4h2rtJxYz-c2tN%sC}CmD=^)Z4hH1gj`U%o248Mc)j(( z5Gt%P$!bElAnj(}A!xhj%R3Y}F>+>@kfw@<-L*w(9**F* zja95FV^_#&UY*?LLE zWgeVpc&_?N(}**hE&^|u!_d!=x|4bVDfl=F)b{HFG`2K$p6zT*)M!@B)LqT=xUCza zuRKnKA3k8RSOHIkI^JEdMW*=It|S-sbfxW&+aq$-3PTtbnD^Pb+XEysF*QXGy1LR( z0G@lK;U({G3L}j@PUjD&qIBwp~3e z%@}-nL3$Ezbd2kuV&?1A!J|)b#<}oK6`6QpZswbBH%;X(x#em&pE?<@g5Czy87Fl6 z=4Yourw6;H-5!eRNz11c2_6X6!p#EswzEN*e`)nNQWl|cJ?cuz{5a;4nw8m)aN7ir zO>J6hYWY_i*~jo+6LUmUm*)Y6uTVGc~i1M7)pUp{ULrcHNi+ zYytKOSFFvUHLuewqRR{4X*^NorCYzIZ|~I`mP>va7JN|-h1n~U*I)o^V8ei9ypqK7 z^yR~wrT^`hxeN%rRK0rGG8uG-FGE|=FzxGVPiW#biS_%f)uOc0!{2|@RtLFoq9A4? z8|)99mRq{7X*bJelVWuh1L(GN4PKZF2Zmyz|ME52X1m1^bl39TcX)3A4jaD}H#)h= k;xVZ(+tus85DZ=79Fmc3XB{WKhJZg$WfY|g9~lPzAI(xLSO5S3 literal 0 HcmV?d00001 diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst new file mode 100644 index 0000000000..40aa8e6ba1 --- /dev/null +++ b/joint_trajectory_controller/doc/parameters.rst @@ -0,0 +1,132 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/parameters.rst + +.. _parameters: + +Details about parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +joints (list(string)) + Joint names to control and listen to. + +command_joints (list(string)) + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. + +command_interface (list(string)) + Command interfaces provided by the hardware interface for all joints. + + Values: [position | velocity | acceleration] (multiple allowed) + +state_interfaces (list(string)) + State interfaces provided by the hardware for all joints. + + Values: position (mandatory) [velocity, [acceleration]]. + Acceleration interface can only be used in combination with position and velocity. + +action_monitor_rate (double) + Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + + Default: 20.0 + +allow_partial_joints_goal (boolean) + Allow joint goals defining trajectory for only some joints. + + Default: false + +allow_integration_in_goal_trajectories (boolean) + Allow integration in goal trajectories to accept goals without position or velocity specified + + Default: false + +interpolation_method (string) + The type of interpolation to use, if any. Can be "splines" or "none". + + Default: splines + +open_loop_control (boolean) + Use controller in open-loop control mode: + + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. + * It deactivates the feedback control, see the ``gains`` structure. + + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + + .. Note:: + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. + + Default: false + +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 + +constraints (structure) + Default values for tolerances if no explicit values are states in JointTrajectory message. + +constraints.stopped_velocity_tolerance (double) + Default value for end velocity deviation. + + Default: 0.01 + +constraints.goal_time (double) + Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + + Default: 0.0 (not checked) + +constraints..trajectory (double) + Maximally allowed deviation from the target trajectory for a given joint. + + Default: 0.0 (tolerance is not enforced) + +constraints..goal (double) + Maximally allowed deviation from the goal (end of the trajectory) for a given joint. + + Default: 0.0 (tolerance is not enforced) + +gains (structure) + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: + + u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + +gains..p (double) + Proportional gain :math:`k_p` for PID + + Default: 0.0 + +gains..i (double) + Integral gain :math:`k_i` for PID + + Default: 0.0 + +gains..d (double) + Derivative gain :math:`k_d` for PID + + Default: 0.0 + +gains..i_clamp (double) + Integral clamp. Symmetrical in both positive and negative direction. + + Default: 0.0 + +gains..ff_velocity_scale (double) + Feed-forward scaling :math:`k_{ff}` of velocity + + Default: 0.0 + +gains..normalize_error (bool) + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + 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. Use this for revolute joints without end stop, + where the shortest rotation to the target position is the desired motion. + + Default: false diff --git a/joint_trajectory_controller/doc/spline_acceleration.png b/joint_trajectory_controller/doc/spline_acceleration.png new file mode 100644 index 0000000000000000000000000000000000000000..98672beedd184285cf0bf4b311b9efa93e916e0f GIT binary patch literal 34639 zcmc$`1yogA*e<*QX^`%c5~V`~6cLaR5L8mSTRKHjx&@^KrKCYhxtHZX79D;nrp6iKK1VKdn)pTc(iyZ6pB#cwu~wYh2Drl zq0Qo8!7K8N&F}(B%vJV*tD1v_tB0|(IZD~s)$yT&>qBdk3-0F5F4hkALcGGf0^Ap@ zTwNVquJQ5N{qqK12WLyZG2`7PI0&xeZEY76ipUuG4=w*@o;3;;(x@OKrS6&bed>{( zy2csy_8_JOmK1R(AqAR88+(vCxvUy<&D)WypA;YWIkPf-x`#tT##vFtq zfj@C~#K!BGRDqXR>7F10W( z&oU<_Q?sbtW6MBBR#qzWUUKE55v(JBA5<9;Y9Gz$l|=Wz0CJqu8!y7Tsj&>mRzFaS z-?&jk?m0C%IlH>LI@B^dhP{EYk9LIOcVH^}92NGV9l8D-Qt8<@I;L@^MI)HqpU7VY zhIPkNdxnRZy{X95z?to#kdJ$nVi%yfT6^J|4o1$$CwUiGLc8FT>Vzq@CM9G#;b9we z7kYA8*9I$if*CkQCM(;hL$b)$$_M5EcBC zo6)3*RLAyG8jrJDvJUAzu^%BqEEVEd1}rQ>Tz0Rk&GOya7Pt1b4#g>O%M2;Qmjcqw{WaFLWm>?-`OFp*R6#d@w8yw%x^pixHMuFNbT zEQ~V{rfI`K)?RYKikF0h0-0y`@a#Gn88cpZmzl>~oqzx(Rpg-E+{jRQ$r*+jYS}2h zRXP(i55{A%uoQB`;VbGs%DG~!*P3>X;lwPHlH5-~hNp~!wzy2yD$A8{Evv!w;kmRl zYB*574RWA0IPxQomjZKRzJcHAhv5p8jNsCXF6m@nH`+1Q`yfnE`Pa+F@Ny(qiJVAl z7vc3Tw{KxEQ0?a$7!E2b=cgZU-C$#9pIaL#8nUUvnmJpDy;(d?&r*@GYoTwoc(Ke< zPb6RcxFIzxjJvn6#ab?gKK%J}ass-m1kKIO^!?271@Oj8m+mb{&Ko515tGDeJP_XY z{n0NnR_DS|@3wBy|A{m%VI>j8>+l7$>KW>68b)gCRI%LC*wl2t!QHNs-gdmg9EX4) zsHlib`qnL{m7z@417#JJ5Hi&P__Z;)qlhpAd{)w}X> zA-mj#-r+VY3(G{A&Fo8ITS|+*+LQ2_?;t5+I;Hidv{Kpy!0{{8!+ z&WpEG1_^O-+nOF95R;P!)y(+PHF^Eybem~x6kl9gT4l1dq>9li7_rG8SyQaiKWO|Q zVR(KFd!&U3mIHHd>|9^M1l9zG_hi}k5#BQ_{eUM=o~Xv&pE_!#AH!Z=#gN~;$s+iY zEr__gzC}U-HX?J&b zd)zYJ_+13WZ50){SjHPsuPm4QQo$ZJiS8oZS>GGPSEIs2`o-WTGz11Z)b@_wEr( zOG|TlN6j-O;4v$n{CcZVq-ix$m~iUk=_$%<&=^J}@hjtV>vgv^_0G=D6_^amj=@19 zn7IUBHI1T?+js8B%ryHbJ$Dhs+n4&e9=I##%2jD4HOLrpa`Z4aXFc{&vD^BVcIn}yz%jIY7zT1g|2&?2{?p=vL+^3 zZTp>_GJ&L_yRUO{+D1ofq`z}GpGz}|3JY6&U&z|n;IZ|-+2?2|SDEglIwRwP_piCo zu!so3-=YEn0@PO@Qn$Ca6EiZ#Byef5u(D!g%g18fdl2Ke`jOomt{8=9>4bxWBkAbK zzx(6I4qT4ukLCWc#z&Qp3@_Xl*7}NPy-bUglM2itEQ1nF+r0&jU}-e?%dS(%d4>p> zAXW(p#*~y4tBGnJ_wQdZ{rvn;k`@-ZZE20afB#OtVol1cUmrv(Y;!?Wl=`~Irp(#t zNzrLmSfQATfSinsv*k$Z(ak<m0|0%@sCOfSS zMs4Wg2a_(2Zf@_MVB#xI-Hm61@-M_$2lZ`b~$u85uJ%Zo>;K zEcJthl~+2ux>!X-YC8Af{$h+g@q+cT9dHR$;qkK<(o?Q_qw_| zcr+_5)WXbUizukeL@A_%_MbmE|3Ktja%%jiC_q=H6>Hpt(m2^Ij(&%$9G~ClPaww& zZojXu98U2Og_LrV)d%d`UXLH&if6kWAC$eo6l2N8R&<|GkQQHAPM*{rtYsdJJL6Z>y(JKA0e&e!4MSwa6Z zA)L_~=v&)!9cZY4fPj^+Rb1qF!NMv^Y+O0YLX>|2leyeQWV?zJfcb?dWqWV$94aU% zXm)d|q4x2?n_(ZfBa%2~(@#+XE2JbODevb4MkgldU~%Sal`=LqHb$MAe3|OY9k$}0 zqR+C)Zq)cl?HV2ycE=QMh2NR)2>}82dHS-w)iASwqSt^rQ)1s?IGCBe9`+-PbJAmv z4h!pUEWSpx1?Rh8X`u}J0SYB?ynW02c&7^o53fCx$rs1L!NK-wX!jfHRJ9~zZb>v= zYG~;kZTX!=Zfvy2TKS#+wuEw(;dgfW3$|BcGBVTtT;g$xDMjnQ&Tw3Ot^M)o@uPE? zm}sc04%1Omfmp`-8{_a?GYz%)`Pp6#?q0@|cg0tne2!J-4X~p-t(ev`L6P0r#RUy@ zwEw*WpAYyzzJ5cs?_yFni4`Wl=eR&xo%4jzbxR{yK!P`mu%-dfBE!(FvH!K%uvKQ< zfrV--p+*1nJ2hsiIvfHZdCLEXhXkd|DwfkzcJ=^B!?q2P#7RIeY z<(jVN^x23cy#5XP>hijSfJ7YLGI_QE&f2dG{3x|1oy|kqr_5!5dKoLMN zOyp%)Xp*T|jGpYXSRKe26X1+7bRy@Ws&tf_cJ1{N8n*Z{4Bk8whdBs+dEG;&QcQ;O z{@03DspeN>qSxs06e1m(f4`S2foI%V93HXDmoH-(Q=z=Qy|)26Wx`y3d59OMjmzw) zRdU%)WeTwLPqk>+pXsWf!)@Au6Bep-PB!~Abne5@Bn#AaV5%+DzJ~FB`;1%+k*2b1@9pLx!{E7vkg@%i$sJlP;oIdHK}jt+!MSH~*h2vD0=7 z^4OVwM=jwk+TPKTdG8VmWm82Ph)wi9OE$82oIFdBtHe9N1-Xsm98kz%lk@&LqV?tMe#WywXtjOU4P{okjoo&cTibn-Na#2W6<1lYKf=hHNAy0-^x-S%1$Z)(Cs! zow-72jY>Lq5eDx8Gx90#ZWmNCWOoY+4aGpkFy4p&Of%mejPLpFr3c)IEa!4$&E_9g z+RW-Vdy7e&{xTK+(I<3h7c0Jt^R>ny7Vd@McD(*2TIXmU`nFw6sAp@iNo!>fU*P60 z?%W?5x<^h<{s3l9EDRlWq3Ex>6HxIPbiy;^pPWp6;`{EFKpgw?8mn`+V3FMa3Cs8K z+ULuz8)GCW$IVGGlq3Lx`!)9Zuw5&*Fm+&RzaU1h`~5;y-c`F=QbvZ238n!lBg?B{ z1kc@sXyuU-v)H@FPAKz5eL4ys_16nA0ZC>GS{3(nx-mCrqL%-zY-s}9@eh-)z38Pu zDkwd0lz)iFALj9gc=RGffu#%h?+BI>CA=?RklM3na|a7bKT9DVmO+IQeLpNG78d~f z4H@{g!Dki-sbTp6|4zNct#;|x!AuUe9z!BG)b;!3a88XCImeMT^hIm`w%!;7HkwO> zBD_V$>^woE8Hst)rCQV_8uv&nJ%8pTIAb3$ox)s~PF^93Wx%PS(avM!eXb@W1I20Cs=V5c?7T9XZLPNK z(yWeLOO)i;6tzpf_!3Rt7sMv+&I)g3<2ycsfTs&3FFF z6A{c*_Ps<(i!HwvlT6LfKPM|}u%YoJBPHG5?d3CF?2beE2jf$j48FSY03HZH(rPAW z6kM7a3ePS+fc+Ddp`3bE)c06a;@2!jB$h!Y$uFPdEq~OxbGsDp+@JcZIUI+moHQ1y zPv0Bo*QE#EOG46fzX~70U!qR)=W_3)n3Ub%Y$`IX-%oU*Km9Gp(J#zOm>C1`6uu-5>++`AVJ|l@+E!3aw|M207 zT(%UbV zvsj<@g+qZnEKDRCd_kcU^XP6XR2a1KDg!(&Ni}jobdgpG6yWQ+y7yS5VPOOs4Xs6L z$ENPgGAT?=bQ3dU?k@|}@>nijlsgl>ex1(O*B3DIlf1k<1-nLAZm=@YP|s~`?rlklUxRfz$$&tg>;iH(aI}2tYM?F_90P1BG9@|PvMuyO{ z>_J>2BEIr7d}_|A=`iEbMH7c_{_Swtga(iA09gv+d_BNMik(TK;w=O zW^QhRxw*LqbK|zqW~h)o|o>6hoZjWbtREkKLYAt6s<6xd@SR;LXDy_ z`>AWs1`D_X@>jYn3ne(O{CiL6cQNP)bEUhpTPkF``PlGApmJeH=W>H*fDgd2_ zp$sX;|2bJrerwa&;bBn)i2)wK=xcpXuFWqj#HFN!!)|Xf`bK~I-~bzCW?=yoE9Bhtt@8(UU_-B z+_wWa@_#;nI`R)I4izbZCz2$t*eM7pondCtpc4J}KS!egSckN>O4iobvs}4C9(taE z)8z~#mY|T3_K^_^Cj0$;QEPEB-a9VaxGuB!%un3E=0Q8B&2mWyZGv zJ=slECmhk&>0?uT$EefCK=E4+(2M_?4cy+|mIM`n->jP$NPYF>x1Lp<{5wqDJ31Fw zj#^u7FVbCo*cyaO#&7ra3Q*3>#|N7$qoqu+T%tDp0GIA<%`iUacJA#j$~wt#+27EH`?%Jk^VZ5ZhcZ?4~WrwgO${kVlas5}c zPKw~Wl~btjlqI?~SI73#&yM9?S3kZpv3na5vELDZfqf3;1Z0})?La*$4BnGi7pqr_ z!Ju|1IWX!!FjAz2iHeJlS6!2EZlSyb5=bp&Soc-Yc3Z5!t7TSJ7D2VVtn6HO9IITq z3ve6KBYAoGgA?N z1be#|9Ce@Q@aGV@E_(A2$Jz03zmU?@q`X2)xHekqP}byoa#-uU6fyiwug)2+2on>t z`@@Hd2f923f+xtKfv=k&RJ_6*A81oBa;Dj~z%N!6{XIL^@-NjHVn`zZ9^gDW+-V-S zn_`Cex%&Z=rI;)g3{<)zk}7Npf}Y$#m&!{TPonKg?KwXZmFQGS7ipI4#ln-}v@Pb#gMGHIob-yIR>VeMOe6M(r3wwC;2Nj%v4ynJ2FRZYo7xuD_z3x z_Z4ZCCQ_FVTMuOf44>%h>CG-In3h-TsG1+PB?IA~1+7ZRrB}ep}N` zIQaO1(wSv{O^?wXSbN?+KK)bT4QyTOt@MRQ!WV-6jGRZe+6V*&5D>W!#GD%_IX_{) zS3j@~2-^tiw@-SO@^gI@KUkU>_C5BGpFX{VWggVf&`|3*_k>tF?6O*J7}Uts14Q>k zM66^{r>xs|?^3~>k>UkQ1MBJ0nCZM#owr1hsQAQEt;^HEIIRRNDMdGG-K=WE>mIK&N$APQ$0p5$f16)>|h;_z}Vp zaOS8-rDTsJ?MMBI+gH}HFKCt;l>CopR|xalih%XS!V=H?WgDmilrT`x|F8b68>>ef z3Dzunx73>i`xu%+{6mlqSZ-FH-XJ9%$ijJdi}URv825( z8hO^9;YVTefn#0Kl;dj5`A{~2W0+YDObos_3hOHL z;YgwdRr}v_!Z(EI+R7iJ$U|lrOj&0aJBI)-tKUbCav@04z8MQ6>Ggd3Pd${Re=MDQ z%zDf`MvXM;?i5HU=CHkU+S07j{XSx`r-e=(KyV`G!r_T+`W+Dak^ zRtBHjC0cK^j%{7_R6qI9LGtk$!3Zpy5JRA86mT2JC`!jM{rUYIkb#bIej^$yO?f?#)Om>i`D zSq{~^*`6%)r;3;!?yZV?Y|_6Fwhi%^@u32OmH*BFqesbFxMAcwR@r=Z^E)NV2<*9! zii{sjx$mu8M2`tvMaK)T>VWle!=1`gl;w?RyXVuL;mILe0^&j2z(9EmjGn) z{dn|qZRA!U7U7=Zz`(&x3j?5RAPYwV-^g-X|B`&ojnnI3Ga5dj^K0auTiysBtoVpB zkNwq}o)gnY`o!*Pnsy1IQ%+I+v`)((>_Bx(!8b*`v#}8z2VY+al{C=+*qW#x#zsW6 z$Fa(P@V38w8wZGQD9HVdC;Jmv=I0Tp4%P|ber!w(uh91L(h@Tt-(BE34UVpNOEhnl zXr>;<6Rf<_8voWS<+g@VqItm!Y{A-BMKhFNJMr-pbaVE#@+A24>`L9`psd_VWa2eK!*oYg&FNep3&sDU4EvGv4IiH8$D$&fKC%;^XvOQosc3wJv zT6048*;OidJ!q-@eraG<;YT2XbxT_^GUo^1pqHxb3^Ovz6tGJEclhV9Rd*Oh^SMBc zgp4d3*zZe1Faf}&r}>Ph6{J%lXJWzvw-Zpq{Q^^UkC9BGAmU#f985_uIgq;mYYjZn z$^-%RP>Hg)VXr)PW*>*5P!3EUQXn;s?b}dkCqSuop-W|Sqz+a|10+Uq(dlODMj(J939OFRf@ zqMMj%Sd}mZ%q6P4h{OOD79QRPx-l^&WeAA*#d>w5G&D2_h4gP%4Fq>=?$h0ynD<06 zZb?d_o&*NQrKiV&xJnOX;r()d>d!9vo4(>+dn*A4Gk#2nRHC9d2-mBDrUek-RL9re7wIAo@C&KdspH&o6YC^`#GkB4A;m( zT9Ehl76&tTxY|y4<7*ZE>FFuTzpw9(nVH$j!5}!SuN9sl<`;YfqphtC@ud$Bwn}I6 zY)9WbXz{%RJ?kU1_q4q>>4}%7>9!QG%c;zPyErY+41So7Pu9> zr@y~3(O+}zzOb<-zSBweYbnX_DNK=&<-lxt7h{nsodP4h(<}YQtO}&=QPKR#c_M%X zU<#yh20p=dsbQJedfo5T+b5;1%64=pTb>9EgSkK=iB?dZ$H&JAC)}86nbFH*DHPNn!9`{PkrP zoPEi7HXr5EfmZecy=ZVk3i~!HuXrC!d4M&Cg(y>>i?oQ;@>FCIZfP}i;m(UIJ(GE? zh5V)>`Rwmx+VwT6a$I}FU4@%ajuKqhO=PNI;kWK$fns^3^4v7=36s;~--wG?@39pF zcf%OOA^_&-^Yim%&P$4jUj3llBv?#EUfsG;s{eG=#>*{-u%2!li+kA$(2YinwuQCT zWUTKj zliUwZVc)*U&W>e@HU89vG4a+=kH8C55- zxNIF39sLfrH>HPz0|W1G5#2C@>DP!LMi+a8*(g==;^S0zA~{(PDfZvj3VeR?+qa}K z->vBz$O@p(yHLnKMc)8G&ofX*IFHNel7yDSRuFjzq-i*{5)^4wUWwyoIFj?+xWNF* z*|6W;>|vvW%_%Eb#Unbm)p6e|QrCKE{e1uD6vdpF)7XrjsCYK1c(&{ud|{i>ap4tlnDfx=n z(*VY^!A$`)J=W~=IORxUx0^NL2I2j>C?2vtQmn3fI^h`W|M6z#TOqR_61WhPE`)$a zbo;|F-e?=yTv&0}N`*DoC)_iL{Oom|Y$4~LrRXVWN?w)OZY^HYaA^q^eIcn~` zfYX{FOjfk^@*I1`MSa{`!>OO1c;BFLS1=rfMf}w$Gh!O|a(54xKQTeGj7&^7cYw!a ztPp9zjP^=It*yn_oC8(u5Ar`}=tH%6h0PN{^upsQG8);x2ag{g0svLW3{Y*?n z>U>O5?tR(vPH-|@p&FGMw!{ESPZ-;THB(&7?UMF{o?3}Ize17AiMp08VTlm7qE7CB znnypD$C@5{gnvg&vlmzGY+}@&Vwm7!qy(0eh5}mG8|<-Mflv{#hK{E+$Hj|SV98y3 zqu-EgXd-&~GA`VrF0fMio$#Fb^@OAQ`g;`GW6DWzVx}-qIFBuv@_1O`vlh6Gd`)|# zNKON+Jp}xq{Qdn=+=k8ce2v>rkm1XO#TzHJ1@*NluSNmPqd)tBh2oXJBeQr3t_E!u zsOh_+(&h-b;7qw_+I;QG+`9A1=P!yFW)n3aCsv4Odq~ePuoMRIs3`4kb4~8wX$M~2 zh)nlA3>PW7pR=M!;|f)4mYcv#Ge=qXeI4BE#a`|J5e^P)WNg6R!O|i^VmOc-^8CG& z1iGOuOhz9~gA`BqD4D&YC0$L_qN0eM_D-D__rdlk7bI0G0Q2rDFXbdgX$R%@o39#eq{k3&RMy1nyX zbv?Hk5GyhqW}5SLYj|NWBoNYo$Q|_t7?!lR89J&vU0LEzv}t&{2A<`kXJ;a)&`I#Ue2*b}hnG*%w`?{vKFSt-!8sT$nB zw}Z+$RHVg6KzjuTGz+YwM_yhaM|_&?9T}`kyMNCT4QJe}_@2ADPbT8MXP`ilepkAvlh1ZE;Ow>mub zGtJRot8D2Wwzzyde|&P1Ic&Avia}%qj|NsiaxtrjVW3i7aArWH;?pKholmK+Qc6l! zmwqbBsNc}Iwhf=2TJ&Lhv`F%7zxR)kaoGt&54I<#r4JO(Vgz6p2IX?LAn(;j2ivK7 zVK{0a=-13(G{8NQuCbrm1}KyXcSSDtSyECcq_<8MAPk_R2Hs1Rj-9G*#HSmj6@#S% zLB!qtqej~m*@t$z@*W`{6*4k1T0?j$CH??9C8mcAQ#6P)0F2*9*};m8ubA$)QNk?; zw^azA*P_vSQXLu$DpCY3(ivQ%8vobfgfU!CktR0J)T~{Oi;D%&>WA+ zHo%tEsF9!^GMr6M^LBl76BAXok;btfwXT`-VtVZhcA`;?#9yusW#!2B+GK-A>BmZ@ zO9_ggy|V}ikRW8$_c5>mEFwm|wI5$fbVCu8vHqo`!ygUf!;M&C^qZ%@SYerItx13x zz?o3k=?<)6$xvlkRL(7P1W&wvzU*|TT!ht$n;n-k=cDmq#+4XXhUdB0eoFZSOB;TA zJmJm-R`_+h<*WR3|)q777L zBvbYwju*?zkcUWqLDiQP+89NX5Ug~@u!K-(-01T#O zCxzwH@+-~y02VBV($)9!I@JdjCbb&d(evrvy9&WE!mca#5M^LzXUB6k0NW9Y&%dF0 zpxC3Ez~jNwpRF1-_(Mu;Aq{ru0~>F+mcb)TyRUDh;Auv3WPn?#_9FR7>TuAB!V zJv(*-t0@0FB)}Rb3Uog8e=5RnT1^>;syPT+nb|i0>Dy41Odt-h0wkv`-x?A!y-mY? zfLq@i!T;r2YGBlXT_D3QF*>w8!F*6=Bo#a%MUg#1$xBhJdqbI{dsMBZEGYbdk93@e zBdkSJ%G!9&c|6U$(;@6h4(M zS<`Q#SZMK7A~{0s$4x(DKm9?Sm^wW2P)#@DwBB z;#LrO5~+umo{+M6#IqTLe6+Z13lbhDh;%EqULG-~tLI7Lngo<)qXCBkBSTQVRh7=e z@d`}K!B_9V+KXVg?w-6rZiej|DNl`GckS3Ji9VQ30jSl5%NJGsTyo#!j&|wyAQL7J zyBIbhZ4lyxL+a=$8?KN=pJZ1I!_Lo9!)(~DmPIkgZ$HYelQiz0f>2ESxzG0iPtxNf z+rmb=Hu_d1Uvn5l8W~|i39bv?pSI$?#XIa%|6+QQO0|dJJBLJ-oAaH|$ zMc^MBOZ4i|GL87z@egE$^f_5oWJN?RS4bqSG=H@5dd%^-&<;8txzO+|d3A*(B%8|7WwFqVbPP)5Pg%lQ$Cbi++8 z8+^BT&t5m!F0HHDjZd4SF&^8o}_KAsxw8kf-#v=bjqp97)izT%_A!HGe z3Q$yUzM1TL@KQH!P(wAQ_6`r@4yz&Py{BDO-)J4ueKkTwS99;%4~Ur@3StjLNXwYf zSkTO|A2A2*ltB+dPni;|)a<64%_qdPbyG|u%7cA!e<{TgpfN8lyL^Uj1U=DqE>>wA zK+L0|`d`*a$2q2~w)`e+hbr%qztVS`%xD%7btgh~Sfitz(vy*BP-2wcq2@9LLGcOM z6xq0d0y?PIV`98Wc!#uzfs(LAvIj-Le~yeK<%#uetSDtM%H^&8I*IT?+^Mjw0Vfi3 z-k8omL@~g#mTu`rnAv}G9$6@JgHGO^Q^b?`Kw~^w-VoxF~h@} zw)yY?7sJx{LVRr292Gx7tK`TnehhvRF;e(dH~dQL4r_ePJQ@^K91GM%7Wwv8-RQEQ zfUt-{MI2r(l+ZD0+$8_DCKFW0d%Oje-~mcjL0-HWwWW9GORBq^!ot3wf?GoZH~ z2Ze_fl5yyY_(us<-$CMJ+AnyI*C^XX8K|N`!4CBKiyvbGm+kK71G7l31X?!;2oXICc)G=W=|4Qw5 zUD$9=F=DC6$;xg99xf;E9cX%B)H<&ob4VJeU(wsz@NirZg1d<8otc>_3R_6ZQQqs< z7r+&N^X84!*SD87iyz!KNeBGTdg&4lY`9WVQe*XQML91F+pBm+9l2+zRes*UtT`u= zw)PmW3q5A;YoaX*&*6>`r7LusfE95#LnkY0BP~3V-oVr`KzVYLEtD6fe);e4`JNbP*RtyB&)ce3R;-G3 zs_@|Rc){p@E80pMm5y`msuS^laPv`i#rSxz!!*3brt0hKQ5UYIL|-$JKjEaB&ps2N z+E;5ewD$bL&94`<E12tI3q+S$y| z-Frpxev7>YXJOsW|0`*G0Ps@OrXi#ifaL!=JWQ;=Yu$Cu|G%V`Mzclp{pl2EYI_lMWRAi@#be6VI)6P<6x-m9i>ZyvNUe|_ZGlH?Y;1fe(*AM(^7rIr7BMBO=KS9+L=k?K+c*2E zu6oq|$rG9>y9>ADqy9Gy5H|m6fG~Sd?|K>HgZwt1FVWxdA_uo$FxdxbNPuhvHe#j| zFp5(f`uwDTxKQZl&+5-6a=4!Te+ff<9S;H=;t<5}Ke_uX?Ci2Gnok&CX zeK!aLE1onnvve7G{Mi1=^{+gX%i`K!d8m#7s-Azl+@Li099>2FFE}_F3O};4TsQGx zC*1$g(}N2=jaPfiL8D;Ur%F5jPloE=PlNWYCXxN$ATmRJ#fliYDuwtm2+BC8 zv(~IfRUJRImn9b4o3-*P!Op665=7;S|V777I>1+N=Z9fXwj0%FpTloBDt zTOjlFf5?UYow$NSmxBxN5VTm`>W5PBj1D1Z689XURjw8jGr*=`Kd_a=34rJ#xbzut zr*k$rzS*{YmUwzpB`H(A08PE!;cN<6vBAXb)d@8%<=;o36lMN5J!L3RKFX}Fe-DP- zm|D*Xt>?+lQK#KSh1a>ci4CQCrI>0*lAFjJorgKffB+>lDDVnIAT}^5>(y(7v{(4+ zTOuRzp)G*>i>6ZHotxC3a+Ii!Vdr1Af2AM4;AV8*xen`n3_~QyRMQ(ZhoZ z{3~K<90BM>6aeIMa_V=y6YA*VLPo}+LJn>_BBDxX-V1|3;`kbSvaIZpF)(Kn5JToi z4gDw}vPY_xdxr$o2~MBgPtw9gD4j?O3Ic_=C>CaBDfmnH&9h^*f9H^-8xPltvLKtq zA}01%h!8qjej+itf9DhBn0$X_LSB?gF-H}2s$rwUm;dXEIr)L7Ix1qX<7lge4}77E zdiwgoH8nL*p@^XYOt|1FUc)tyo}0zNbS!nJ%Wrta^w#M0D0C#X*73j80>egSu9 zOP>VRmyC>HCoT99NC|_u#Q&9_knzi?hc?|>@85#r;^G$+;!9F*K}XWR>v+r@ahwx` zo$BE|AelXc@Dq#RJg9oG;>wYpb@va(=v!@p*a8mI;%TBz=+G+#7@7-g7?a%}H^jwJ z`@VzDh7=9_Y=N?Zj_(+10}h1 z7Hot5MGaiJt}nB;0uN#uL&s+Z<-7-fDIQq6E&g--X$hkO8tCCPMt5lOJsB!Bys0bF z!K@scb{>@+Cys$hOZY^X+4I{_L)cC7&|j~14!6i4ng>1_rPG=!yRA zB)C;N`UR$INJK)*QmtM^DnKqUcE7GeWBtiv75n624oY~@srJ-yaa}U>PuA-bcFd%o zjM+5>9Oj2pIJVE&3MOf%A@9QSC&3Q=gQWKM_LL7_dM{b~_smS4(b?MPK?A$p*aeot zeOTH%8-z3R2Go%MS0z%2%baqV z{#EqFvErF(uNErxhy(uYJku}R*XG^s(IdONdI?eSum^DKI%iDKTHgMu3!{%qN}{5@ zVr4X(dY8ttXWk;GuRUs!CVBdV!TW`$UxVtEj=se%W`)^kTN$$&SK}JQIdpMZh$5D; zV1cBXJ?|TXCOj0R{Hw<5=r3+h*STP~oSinD?m#y+qBLfKVh$Oea6;XSezciVdS>G9 zgOuCz!jA}=MRIJp@!mel5q}oeU44Z%T4Lky*RQd9F_{XrY&k+yD};LQgE14CAR>Ba z_69UcK=LTZuefNC0s5gpj5p2J6bl*aejr{xf2rMli)*Ke@1RCLxkW_gu5nWKMQNN# z4O_(%U&7!@{p!@PzFS(Ay{QS|-0}xMax{x*Jc|lJMRRP$y}Z#55jl?2#!Nr@73NuB zP)vgM=%{~Wp^XLS&%t`k{v2BLvR)ReBJZGm+~iN#i^e;AG3w@wOrmLYI^WT4>P7yT zCYo^5%L*$qXHsV0@PnGvMAwcMBeFq`XwPj?|6>C@r*n=}{7d*i4g?VhQL&vQ76>FY zIaRn}beDg=ckNa@-r0*EPFgJ8HI5NApxP`hCmCEbQjo@3T%&M?7m<$Rmq)qwB^a<*y0SIt+j zq~`;$8Ov70@()dB*(14l&J^N`+xV$(+_sZfXJr$Zq0{-OlgOCfr$2opX#J55zL7vcW^3p}4p`A%P zhXG8&&RVAhe#nwQr!F1fjaK0Qg!`9uJ|Z|7g@cP5z^1S=UdiDf&)Nn3!Qf+`gYH*| zXa|DgI1gW4Kw+Z`p&X0;7r~b4K9s_?U;gx1nDvkb`asT~-*emObFnrV6l2e|g1l`A z=&ZLGHole`9UzkUg#yENduON3wzMfX{ZTeN8^0jBB;YhJb34A1j?8>uEx-es6dqJs z5H_Fe>pq;QCWNH4@UN_otLVDJrEBE*karb144Kb$l}m1^Id$@~s2=agf>bIjk0%xz zQlRvu4SqmFo%`Zc>N>U&-ffLpp4*40H9;66D-)M&7^0~0s$j>pPd}Q`g!u~85yI` z-Y{wUY@#JJmRZd--|+o4hmF)>KsujmlyL%(bV9n{sTA~>0e5zS7C-`qYj68Z8ATmY z5F$ic4$9oW>lABO${EHWXbYN0sKM$$n$#>TEP}r(jYN(p(Sr7Y^zGQDSL<-u?xUvU zx8u#uT-p=AT$#G$@OO~cTQlbLVs)X4M(+CcDW}Ma&OSU96(s#533Gx9X8n@Z7SQc9 zCGJ|;+5H9$ZBf6k1Mth-)Tf5MlfKuGoq`|=ChD!#Fghd}WK~s3A-JY%GE?t{ufzjr z^aHn>Cv++kGM0kMf`k?!yZp7<&enMX>2?DefkNq2KMd~(B{Bstw*u|$2;qfhfcfL! zKem7Tz(66kAzEU5X7Q1S8c2$M#ZCU|?m+xZKg z2NsGH#cqQgOKRx*JE!WduHCBK#pr%U6nP4zZHUh_xB1#^u6>XzlJlw#P~trEikNw- z=`Ew>A@qV>I$v0>I6U90d$3Pr-B)7-0wsZ6um}&$^|{tzF?+(GhTmGNK@@vUi5b=} zr|TcNAa*cK3y?$VP@ivkE?Z4aFC?u(P&_2~};EcZsHSApy_Qc6)8p{CjSKQUUaa(JHUHyhi$^jK$)k0U5?{g851wcKN1}T!$O(J7kIPvGAqPKKkqMV%L6&o36eSe1(Y#5KvWaW*N zT$GoIx#gTV7BgL#=m!ZbUNg1>~edc+Ake}5MKHBvq49jj)wBu0SSf9=VsH> z(?~sn1koJy&4BCa!(cB9TR*kSw}E$0nQuJ>2n7m;2|TPQe+i$%40uqG&fqe~IcYE* z5f2c&Qke%29^@FBLRmb|C>{o$87&|uP`?WH%DbD#UFX7~)dL?oFMc}dG45aLpB-dLqnWp6DEkJ6&toNBFr0hWu&n7rHYiQetWzS zE#e6A2KdGUB)E`#(@z47AGFDI-hTL&10b!E=@LFe@E|-KE6Y?Y6@c&MKvIYhC)pEO zjT2XB|Ip$C=H&FFZw!6uH<>&(!Ks1p=WMjdbn9E|PqT|=>5(nwmX_JT!2bGP7G-?H zcmFB)^4-^Z{VdhcKZ7PXJNf+w>N{X*`zWAL^nMMo8k8AOPzQTF=%ryAwW@AC0b*TylZAtE%Q)cES?Rwt>j>SJXB?^hUeN+ z@tL0hz;BC-gKzJz0}EXoTnjjW-DP;jO@)gNuF#(}B51OL_(1dtp- zeFRUS_iKMS{(Bn(z|NU05q!&tL+u_DS=QZcJM4@l&u4zg9?p=DpSHc)()*=!dMmir zDg^0JRjNG1fp6Jh^F8^gC4RV)UHj3>~*{m}slR>t(0Sr9sC5j!SFspuM;diZm$4Fso^QPj z+`6D9MyI@KhmLCWSn=sRcxADm2~@Br6~1)@DlL*178MhlTkLsezBvI~jO~>kDDrHS z>5NJuE`fC)=pGE*y9&J*4sMYhR0_cp(_&Zf@u+xuY2mco-JAgjfRJ0k9=HcgL>$6J zPuY4_x>n=a#P(i%Njx83pF#(&17rb_@A}#k5>dBkp2B&Bej7|5reMh;A7a5=!ys#Lk^SCbzr z3X6TjiT1r3qATX{b0W{xj|_%(rQPXlh&5}?v@?tZnNx^sS*+)dQM@6yJS!$e@|!=; zP64_hu%xz@`c*wNVk0f$V_KULu@O3IJ1KNeOb^rxb4SMn+qo?-Sz8RI?T0~Hqe_9W1%{gaZCmT2X?G}Ci3A?z+k zm5fvt;AiIFm&cy-IP8AxpCn#inxN#-F)M4)DY?2@3oUdNz5a>m=?-NKVdQqrOmyQ4P+Ok-j$tgc=N7`o%*69o`_7tnH_`3 zz8Tmv&f|)kKsm#hvp2MwtG%o!c{=5yl)U#v~MUt0a6 zLqSTvco-d%<%m2PAA;`{BIb*bvhIH%w{gYUHT&9Y^m(=RR>S0V5qCm1I#kE@qEB*n z&XQ(;F_us}Jhh#vl-+u-lxv!h@aQcG!|TC{+a|L*0unRDtxh3Rj&yg4Mo;LR#ty#? z$z&f-|I|PgNGCldU(vP3TV0}5)?ilwU#-s_omv`TpW(KigcCxcu|p)MR7 z=}ZBnOPLK(r@pVGqYKNV6kF^XWbOkOki`ch8o)Yz`Cy?!A7Lc`~hexZF>Q69X6ln&ciR(j(F+0M?7)R?K z#!(2zJ91b6le*QMi3Y3@a$Crc6DC5ManzYX3*&+-uT|jT&hX03RoL|V%R=yq<0oQc zW9eqPw;X(OY6J2H&^lDF8x`KTkqdR<`{)8N_ymE{9ay$(*{zekAK{M3$sY&m9WG;M z6ybdm`zc*j1_NYK8I<*XO(&Z_A)QGT%VQb?jaBtE-x~@i=fTrWnx26nY2GDicb})# z+Zi+&+X2WxD%zE8*Q9t1choMm*zhJ)mXrU<`+#3zWsUxOWv9b;lb^&KQea*o>aYx_ zZ|BqGw+6?4F42ElT+F)EYW?E8p^t8m6)@4dg_aa1tI&~Ad6eOJ460dFZjOzeMvcf| zc#QR{L1BGRz**xs5|tb~-W0XuK?LXn|184&2E89+qo4pINW9OHmJxrzWt4FnpVo>Q zE95z7!ZJrK!VuyrQWP{u0~YWZ&qb?<@|Rya0Jp2Di6C}lk7EA}*pOe?Yz}?>z2LaHxp|WNj$G%l?oG~mb2J~7z;b7I{7=ME zqnX50oXM4mR}Dr|BU7H0Q(%oopEhzTT}%5F zu5F>I_+{snJH688Fn)n)@fA@EG{MV9R@`igiiGl%3(QHV%S_#?8t222NOhK;+=_|{ zv3{Qcgj3YUsO#$ud@V6zinV<-lw3bNmb!uSSDme}u$yk4#oi&F)aZoS?ELa)0kVIF z1)N2b=cwmsw+t3tS;qtgrY9aM%5?)YU@;`6!66~B z>=_r>1=MD|%XZtbzpu z62NOO@AQ3!fPet70|6-KqcbASos=1`Pycw56lux<4g6ilwIiiq-VZH?*$3czAdq&| zBX+5w*rhtvc+iLYOpblsjJ$&QWRq9I0ZFo$7_wnv~7on2_}s$Q%1mo2dqgt-EZQZXf<;SQL6dmG0* z_-avGZ?CbeQb!LmJ~LWPeA!mc)xG6@|NCf%hcj`NLNh8_-U^F#Y2f5JN#9?DW^P1d?f2W;z-Kt zs;VkN?+9&QGc#f>y)H=gD8B=%h8dq7#z8SCP&7I`OkoE|`J0D*jM5XZ{qNgcuK@Vv zk>&e2kV=rZV4lDxRyftW>e5-DuVi0(x^F8P&Y*^sf$`AtZ6_UxInLdFsBl8%bGIKy z1GTg)NFWFdJEANzDn*190TZmoziE5j4aZ?@*m;$^07&5ux|O#w6G z8!)8@3Ot&wSFSaH&KpSUIjmG7h{jg%P=xYe=fRQ4P{7zv54P=-S^e+!H06y3tAqBK zJS1KR9Ph|oa^*)>&(8sGxsQ=A9N@INvDtGe9%T|pj`lx@JQC%~419`eH=Cxb+Ovho zrHn;7x$-fmmqb(@i`Cc-1NXC!v44zccJ}rL;Q^b*con_5*FN^Zyel>pv*H%cuP@m- zIXoOKvm@!+a^Z&=2tm3NAO9J$5foLKPA7Ia!2*p#qy@2gF8!t4h&g>IqAy@{y5sf* zQhJh(Ysw&j9-kuhu->KC3J3-tQ0GXs{o+&ciW`zqWJ_<`u6q}PpWCqGbi9ASjmTA- z&i1SKp*Fpz6F(1L+h^bMD!kq^^#TJKtac#PHx{v~rKLDC+u}nto427ML>Z+^$wCSL;F5=7R;DVaF|LsnxBDkCxeeZPk ze1g5 zK0e%hZ#C;|-Bm|3VqU8J#n*ja0WLU?#0ghRpcODhT6b4}$78h1liLIp991X%CyW{# z*dW*wa@Wl9Xc*es{29tUuFW!f0>6e6uIo!6Q6TPJ>Rir`_SYg`FX^dRFCB6uOun}Z zheddVVs#Fiu`p)hpp{JU3Vi}N>9k_A^c3>mqEU1Y7>@sq$otp8@03Cg>MX5&e{SwS$p&fW z)@MDOgBwo=Oq|a}#&a;T3s$TcJ*rSAb>x{rL2>QXWSxNq|JARypnKA1&x7kloPnC> zcFE8(b@8;9QF5YKClCXdVY8mbfS9->O35wt^lt}X`{i9<7cTa0E}xt?Ys(hFl#cZw zlY#AoeZS*9z;Z>}=Zwhx+xDu^nS<(pKsX@RlCAM-=FLchSuj4aF|StNFe|r|$)+kZ z`t|c7X!e~K)jr$wjmtRvxCwJl+g!&tj)e*rKB|n}mW}n7p1&Z%=e=k8cZ;K`nW!X) z6$X{Rwst9~4V;cwYd1?tvAepuj+8@+C-Ch)r4L(%ymCc&+m8uxMb-T2JTjTJ{K?ZR za>FllvLwgqA4>AeyJ5NWPdM13ULOjFW9sdyvD4X^`M`!;LwoYe*?DCK8^}-XX1(|5 z`k7)ALmeNgZZ3E-yk_-**WlAOPUJ`U)b~7h*d~?2RNmvW@?d<6QGOw}`t9NATh4d7 z`377n#Wfah6bP*K{KB*PK$aJ|jo}GnhCkS}vt2j*Nk;SCZR#kJ((9jZzySiywdjXW z$;JiwRT;%!p#{py%Hn9i=jUR(40)9>rQz#oXq8$a=AWnLIaGO4^Q-KKNaw#($8t7R zR5Nk}O2Nk{AAb5w;m7(ycHW_8uEp82FWChLH02ho9F#M^J}o4LdsF<2(Ur3X=JN<7 zu03YD$cQDdfrWoPe_v_Aq;v3r`B(VQpc2y?mVIw}Uvk}Uc{C@!t4qAIb(S}rW7D$O7_z@C?x@FRD9^Ia`k}OYOhNqgMCMr|_ZJ@gdC04l4*ANM zHIu>VJN=I9UDN#~kLC!d4Sib@>WJNwyx4SpPfU#3xwyFz=oT(_VmVSSZJP2dMWvU7}%qu+(SYQ7dVkRKBDBRZ($B7eJ@ zWPUEmeI{vzhVi@kL1A3usf2sk@}ib817#Wh6U{^lZHTAb?|~oQfNfo`{Scfz`+loW z)<2rY3lBae{k8ddXfiuz>%^oRH}_1=ISn%?3FSt%HV+Z6TFoz-zMN{*NO3M5ym5M5 z!}@OiJly{}mi}kysbJr)PHu{-?_O2Hf9YI$$cb}SF8oQo;;pE(F6>V#Lv2CK#u#!s ziS2}7#=P^co&qSp)Mu_YZ{8&5qrsRW5qRJt|9PL$VO{W>mEthyZ#?n3sP6Z~k;mXQ z%ArUwOZzE4p%&a`Onx)X`{awR@4WZT+B#zqTINn7eY#9p9U0VRQv;cEh z5#=%+dXas(&Z}VSAjTcppo<$FmjVGcNVoWe6mk{3Z;JhqGU&oSt{tgA75A$lh-;-V3;v->)zmV{WrGgT~UgZ+`=J&JzEjoG6|mx1?=v+A-e!14%flASxCcNkhhZOzUukd z()(rNyVX@0J+GY^TQ=+XY=8+#X@&9T8rYcPx4LWyO)_TDW1qR7ZK>wz`<{wuR3RRi zLXIz8P_3PG(vb~O_twbYHNH(-#_Tb6BA+Fx{wEw9iXl2Wj}&OxYK1`W@Y0h=eW5UO>Zu=D*Kxd7{K<;m>Pb=W{^5IcwSMpWvP{A3QNvsISB?b}ADo^Lg@h{V^wdubOqw_qQ9AM_qICNN;G^mmA-=_!t7B zcEpd?7&%2;=|ZJ=i}%B%>rAhK;iDnn#}E^j3acGOOG@Y`lBWy{@FF#O*qv&6WlW1_ zj2AF5F|D=wAW2d{H~o?%UFI$yol@@!kFnAK@vRp8h0m3Z6-MND%oD)B2*FrL@XvTgE_Qe90zz_fyvX{3oGKZH<{7VPwDuwgS8_%*B2 z_E=jd=BFSK(;xh%Io=ZqFm}mbz=noiZ4O2pJhE+C49naWu=@nt;8mIe(iui|81hB( zw*H}2_fEXKx5&WX^criecqEL5ti`tB1t;TFPdfSK&vP^UYumi%hV2HhME-2vd|-}1 z!%oJB%KpT3v@3t9*IPa1de#7dB@l>q3vP?-sR1z}Yq5%_s59(h49>vT$|$8JfYUJZ zMs@4fX_RCH*m%5H6J6m5>iSoG2LYLb$T_0m zYQ(>prj7%Rr$i|jjL$)1?PH#kl|?Dhjaqh19`X<#pNDMi74QehbU&dsqF`;-sA|xI zl9=`J@-x@?Q_NK0-tx(PE53MRjwcR+PiXci1p|m)84#XJKq23KiHP=FEMv=ORiva+?e2Hw@w>^0`<$G1b1j5WDfaRnaxO`A3? z@=*FUSvI#|~$UArW0)43OFm~S_h@b`YP%rH~TZ$rRF)vPJJ)Bm4 zZvB^-;pZn0tG|A^akxYfZa#z@)H;ePnny3M-O8MD6@)V^ObuO)i*7$Kz#}C^(@%gN z6She-1RXxGE16?63dd{|OT28j8E9Y$vw%By!CFnkg*5DnI8yLvh#QUpwEw5E*@))v zeQf)F=j5fEk2^N%9I+R_e^j93*Qy8X5S=2NaI+vDCMZKo38H{goZxQoBkS4@cLYH1 z0kncLc7F9V`NJh+f{2&4SC2G!$JeU!4CVb;i-wCg2{;P1K5Bh`m-KY zy#}^nh9?9b0HXvQ&ykUlRy^1vIW_3#sr`X@Rx+I*O)-Tq6Bfn*Eh!0ia)RAXs2(m~ zzwXCvVXatA8)xN__#ag(gMn?E;`sMlPVUu(vL5sa#1DRWR3h8L!}OIr%`P6g3)J0o z;-l<@ouKT*S2q8^Kz%3u^A9#0ZD^}ihGY~0Nn{`~oxu@}%WAoQ2w|RsDz<9AhKp+Uam}2SOp-+oiRak6XK&z&A~m`4qTQ@ z$mYnL1OIFsXY2;jicmQy?f`C2!}dY^4SCqH#F?NAsm-wBM*IT|GkTvs?+ITs4eG*e z_;x^YY(NGJwr>dv3c7$|if}8C4!5yQSQyI2qADb4v9EZp6fIdO>L|OF%u!IO-u9fF zu#gLqGdLW^jA;)vE#r=qu-cs^yh;rzna;1R96UBXGzry9M`N}$x)Lv9q)?$!G!t#~*uAGS zmb$b|BRmP)pBj2XJe|fS*xhhHme}QaL$M~fY}pLL3jwz69Csu|H4W{-it59nyN%0D zqd4q`dJc~J_R_at-A<#NB(N^@ICsPbg*D7W_nY^-J(6@)6iNcN(FE6Rq?a8x8y=Qo z*xcvy@m->LbeuG*$Ss!`ug4kYhe#K5R2c+1w!gYL7gGkvgU+rrFrPB>5NU3LqY6cD za$o<3#cWi-5}?Vxd29{t&|&*nAqG(!RD_Z50g-~(#I_~qhB=h@8G@`aa``kf4`?lh z04*ZE?DS~&W608^8{}T9bLbD-JOT?8G6dyM)=WcfR}2WD!xtyR;U7D!{+o{FWbL!J zbu5R6w%U4lPxRR7^p+*hGIzQPvFfMfb_2$qlU!MnCU(EU4aJXd8&++1|>-*(ZCK0u~JVliGZJeAr>pAAt(b%ju?b+xPb; z%By2c5}F1K`3aa)!`$Gv!Q)k;_FIz_p|FZ5gVsG&xt2d)JvX7p_U^Fny=0>r4TIk~ z4KW+y;fxivI3>3Kvtj5@&coKKvkjYNp^>Uw{2{8$8=Ktg zxhjPo?hF%S5j&Oj*={JUHsH7ldjzJM8TefEyB67@y1hwnwZ$kX`hP61G&yT}C+BXZ zxCj0G7!D)nswZ*cO@JmhTW;9!XFuNxT_6aUba52j@|2*n?x9M-*-y|+AnC@cyB)Jj zb|&lVkEw1r5%48+1#)5%+ctP7H*O#5-)xe-K&uIx@?ztrs}`Yb`DGSY;VZW3H&Cm{ zM`u>l-p0D+6GwKS?kffZFv{8aG4)ra_xS_Kyt-1S@0CI?5&^4jx#VDD9&x&NkuNlv zuaLuU&o+x^D{nid@9yZZxOwYsgxiAoI^R@J?RDhZ z9P>PN0~aaAjH1 z!L~C96vZ^Swc#%wcL;Np?zNY&&h5|k)QHeJ<6E&G^0p^=d6asI+wRki;2dOe1!qKV zJ5=*BL(#6k>Y&vnJ}wXISJyRk8ucPQ7hHZWCNVr+6h9_FDY47KR`Qw9Lq4mq(*{a? zn-IX}0zNzK_-EDebxt68$`De=sxK$xpxbT~RZ}0Et9x9fxwHGz$tn~MP;Q5`akQ&#lo)7D=TMeI>?!ycz9|mkz*|oB*N#FXCWRAsah@Q znM*;3+t7`>*;;)H=c+irU}AQ~_TM&X&yIB)eG*tUbj>6q{>DB>`ScixEMAfL%Z69e z)1@IH9*~}cV8v%YoH**vVCqG7b{58wk+itWI=guf)(MQe#;5ydlCl_#&NPs7LPA2| z7tUowDj$06H?fHg83+i#kWl<_#5#DkVc^p14-~iUKFgR``HSVEsNM2JznD)oRm6%j z29Wsl^2Li6J10+Atla1}WC_nVMP@yyURbxGNs^^}lI=l`!8~yeS!<^nPp$UVukZCh zSaml2%cXC@)j~kLt6zoNOR7wcUv6Q#e5YrlwGc1D^x!0;9t)tQq0EK9^kJk_QFC@m zoQV8-aYfXYefxN(<|VkJ-<%qZgPUR{bFrd_C$jC9u;{eR{7&Nw50ZHu!fpWXU}l&c zs9koWzSn!FFEoL5chy_4xKTmH|OOEFqHTiWio34)TbT zFr=;Nh^5$ih^)j^q#0iI-`Go(2EqaiG3>-nU*OYCoKQGMc_o6{IQ)*OYtLVAO2&d|Ik>@Cw6Tkbln`zYtK zuv)}`fby02>F!^G)nMG*NU@0|x1;u)5<5tNMm8n7q)PC1Vj$x5&MYPdA_xSL`#>SW z81Y4t!(ti{b|cWg?A7sKX>UW5C7HZD?lP-!MJvvY?)v#=b5y;JINU~s#5G$l!Ys(T zLms!#!0dM)^#cSlh%lR6We(?>5ywE3cws0Y4gh>usv+&;VtuWoEWC|@MH}Jty%Ktn^-mZp8B;wQC%qC~S$JCwd*NM59G!?06@`&0dgs@5HOhnkg}--8;?N*JBk} z=||9IK_NtyAb=2*aMKusir>T3M(nuKRx%fS6L0;igziuXKE80rBa#kJBrfhUmjzk!^TG z(Uk+0GbKh-%L5S+IR1wvW}MrXNg>$%ZtAvmER0#7qlpAfVc}7;!NMi{{7I81!R26zS0EVLW4av83~v%z1w%5N*)RDRu|r5{_-`rC9apC8QDsIw%nr@mnqP+ZhJ zF#?0D0z|n}1chVnr;lh@W&Qx}6$5w!PCg$fIf;p8IQXCa%Z;aLRe-)(9{B*la{kT{!|IY6bFAYRsAba1@o45lbGz# zKR$f;@Ok43@ORLRj+EbJHZm8<0}22S{QZH*`~OGOGz_#wxe2|(t3~I{dB$%U>;ntq zF8^?gP78yZvBoT3t|Nj-7=(reQ@vu=>2+Q?4{U<`=v;D{JL1mlh!amccw1BV_q7nY zK&_@UcD{6WoL0(>MY1KTWx~r8W*mz;=BvQNa%9C3_kfhx<0YN9vi?b&ySty|qaPs4 zLe$MZuv8pfznC^ph8Gk)v8e7cZTNzj!b8{1mA-OBF#m`mY`R2Yg1lQVtFY+RM@FG3 zO&l-?gVB~(+QI~U&qz5+L53F+bxCO1w#j(!@7uYqN$ZeF0xv8iKi#RZ99>N1-9C*9 z*)E+$kJoHEqzL8ZpgVog)7H4H=Vqh$_-Q?vz*GF3hY{9yQ2KqAd=Pf={WX32tC1l& z6DmlU@eoiAYl<-NI%~{+2)=37vLX0iBO|oF9p3IFlYcPJrOMPU`R{>FG`XRg1xC<( z_az8EI~x-6RC4K#?Tfyw5oObB1HY45#RN&^h4Gp3Pydyfq5FiLjDTey<_Gijzcz)E z{Xg8`650v6@MCR|FGz{~_phmCL6CjkF$5^nARv)E@{%vJ!c>9(@wWv?hW|bP$TTi& zzrM-{;n`P~g$|E^%1x{_`zZ(M4Z$@CuvP|{+>n{n__d<@Z*x0?;%ZtDtBBz$EU>s= zAcGsw;kveD$p|bM=FhUY78u|m1_2uATQ^$Wsp^;ws?c29~9}MkGv2`Pk zBt6v7)@V^T%S}L=Q)5lu$EeJeKv0Q;g98v@G9$dZGv4OHuloq2aU9qf=ZXdq`$rQP z$*2R!4cY5LD^|q6!%HUPe@DePOeRtgs*uF;UPjuN)Fhl6X3wv#R2_;|bbUxUC`Fkh zFpGikipwf?gYF}Nd3>^h3}0yB_T6^+FmnsBJ}=!d?hd;gjY_AeCi(4B>ODGtKwSBlJqwqLYq>$bCYbJ7X+AOr=aR-7jv@EuI5ZAEVQ z77R;}tl#Y>RMT0g>rlXKs{M<1BnwjPO^e@AIN#WkYy7N_>kOJ=ZSWt8{^>_%+s zrJml_*6BFI{4?HP5wB)1qm)VTQj!0pQurK5ok(}7X#3cK5a6^V>#HKZObBqz_vclRq_r^=mXGUV2bJ8&05@+XXEOWue zrlwLTThGo{Mu4mehMMmxm=y3F6M~$-1Q@jnYU6##cm_Uch7&jQ)NBi_Di07%-jq5v zjFzMnIBp4miy4eG*Pj-KPtJ;GyN_m~oLLBbV7eEO&iVMD6(ty_vTPaUzFcm=;G+3- zWKxh0rfG9Qu|1N4;n?Fep9<599P%)GX%mJ=Re|IaLv$^Kg`5CG*N1x?8SeWeyl&lF z6Y^Uu;NTDj=8UPw^D&g-v4j7rSmE(L^dA1$t!IOUM&j2h>`)~T#M`{F{P{-Xf$o?U ze!_puBvLir%ph^}fByErl*iom&cOOw5X_o(9@&Hc&lzU_>*wz+#v29e0Z+ER z@1}7%5zsrC!PCXuKw3nQIW*I4W59HHntieLG<1z&iwtc;j>~+l--t#bd1U%eYycdT z`VV-``U9$>nCGptq=NZ*LDC`9CKU>ML?^0WL5A2l4pBBxsNNuizMfxNahHa@mc}?t}05`UVEVGqG-D96pU0 zsBR$@A^#rG?jwOc7@4$gE`p8ff|2<9iNrQ|ECGhIGYA-kFiF#_8Fv^{p?QEql>_4> zM&J`d|50Oc)8IQm*i5A4$NgG^;5$YkMQTD4MldY>o=n>&KjM4|)WV!KEz4YreS;=- z$@oVwSE&$66FrRi29Vs&@xYw#`zNtK1V_+y4#tBIc>{uclNvdgL3UzHQY_RI3@?&f z5a~`@8}fex8X&nLwo)%7*^zI2DkgKJQ;Mc+GS-0a{BLYyK-5>r94YJW4W<;wHou0d z8Jhy}A9&E#AA36kGt}=Z(>T>|Q(53)(vo6h$Zo*Vj5j^1~rmOll12@;I}1pJMykE$=y2Dxa#tOM)aO*UHV$I3wpnPy+9`xG%?tno`R8E zgGiJ+ckWz0%8o0yimn`Qoc1yM*obV1>$d<&!`X@BFIW9FfY^C-k?^upUyeh2U zbq0;Ior1VQ*wT#x!FM$9F5*Y@W@C8K&DpbN75)<&ibpgqj6+u!%mgWRkkM;Y2m~L` z^cm#LQKN+VNqT>5E-+qlccU9wHJC+<88bD}mXJS(xN%q_yf7=wpdO>Cb@dXFgf9)~ z{|vA@Sf4d95eI~Gcy}$)!!+(z=A#P}EFi@qL5gQc*o<0Kb!ZZnqvw(^H_(h=D85M~ zPP%k(ET=)sz-hbIFc%SD_mw%6XBot8>FyHg4na~xM8pC?knZjVML|SDx|B}o z{KlovTJPHXefRP0Ki`jiY!8<_3np{kab07aHSW;6cjQQk7>F~4{so0x2xf(i}Vs0BcIoQ}c*;pDeyO=sYu(Y?k#4W(h$Hi>n z_c=uyCi}OEnXn>l9E? zzDZM>CB0u^X2y_8C(l7h)cASlp#DB~zm@HWv7E)A;|;*TT!rSidBayT+D=EAvWAA1Sb3a=t1pttubD z%zWVj@y2}LqaeIihmbZQd_9co@<`B!4xTv&c)+Y%uQ6-9l1|L`1LPY38+r*+Z z`#s;K-{N|%l^raf!tYEk{q)`4^UQG?hddP|=*^_baZPGyBRcxHCzrD`Z|mMkA;*Zs zQv2czSYWPm;Tt^3AjUB3=95I@d=_3H3h?vXS{fg3e3Z%^bi?uW>(_U4^{VBocb@sj zg`|ZMmMcd4E*7M>pD%sdkj0FX>V9ytcAzYFwoJbKTUH}CMz`PD**PdX`=Uk=Go?Q> zVcW-#k{%w{Fm=Pj)!#ar;j)-ucjzzpHz@QzrpComRDDS~7%|{=Tw_U;W@Cu+hnff_ z;~7qlPd3+cwX%DIbZKd6RpPFn<+yU?3dV1J-KngiLVaKzA777!h3d&Y?Z$?I)Nh?C zOFKi&-x`(7z6IXN|T(XBMZ^*gVF2L3!6gAWrX9Q;~0bI z?=*zX`+9qOn_|gt#%x;ON>S!pp`=jLpa`XS(mtj6Rp9ejaoYRyX?OxGyVB2g%2J35 zPi9>CjO`aGr8kwZpZqk5+?oGWv^OhX?w&|fON)_Y6y6i~f?R9~qTO^K>}9r2QU_6dnSx$95Wx<<#u>2t6aNHoit zHmeTZC@Fuu(0i6Yo@8%xvGMD>>)|$+3>$Doytc{rww8H%`0R%(@a*mF-@ku97DTH@ zo^r<+c5R;Tk6RKVqB)a9#LZ3<8$dFmJ3AILN5$20l( z*FPo6ya#1Zxx|Ig>m)tu0fv5Z@zG^yKep}Ieq;S##{ z%*6w%j#pcEXw;Lb_RB75x$m#Jl6&r&?`AVn(}c5fj$P>1fMrQJD0J}UMYa#6x)s@b%4@bnjcN29hUGhpwTBZ|xkEbYyT6U+NVPTQ#`}Ko^ zbYx@%<9Gb@H4MgMYbmm+Njflkvhfj_@yDlZWlpn{Q;t{n_xJOxdN?mzetZAD*xlXT zaBb>ina2j_@l!$=0s?~13F7+pO;-d2s4tm!H02w&8WHI+_kC-NZAJM7j=-$`-NqrjteFM?6;E#2aK zVd0#=#zP-@bkmU#N-Qnvh={C`cX+?PQ2o|ralo)ENe1I*_T{DF;y|&G%K{xXHa5vX zZ?-n?+``0!@`CCkD1GzZHJ07%P--SV#a^)JPQ9IO{d;SxwaWcB2J_{WU~9U{hdjrh z^0>ITV|&ZBtwF@J4!?ihcJuJS#+29&2E)5e!orcA?syZ)*!|-Ny>(wskPf$8`}2#d z-)`}Y_vahGxaQ1h)t5tBu5;Yeb5Hs9&&0V%RA2+bh$&!F*OOp=jE~I<}dJhKu0Kxuo2l%{c*|%W;CJy3>^ZUi=3}5&M;~`jz$dCi4XE;N6Y+ z>&ab-w_japYtM8GfrF#Kt)Ed%NGh>bYuQVg3q_|CAg8#?R5K4pS6AI3emuvv1@I8^`g@^5^(V_z9&p zKPV;YE$pnUIPFGjvQPf3cKk`iDC+T0JzFbw-bE>FDTixPQMMF6sEv z(&O*nRfk1}3a&ZNoj{v_B9|-(H+Xj;ZycGzKcCdFWM<(+EwSTE>#a=1q>Ut255vCJ zVXQb>g7Lc!WZw%Wh=@(nmhvi~4hdv2XwKS}uH|JEuzmd0`0X2IrbcedhXkK6xah`h zPtM@ma&d9x8Mhukd+uB^+yPV(o?p~f&-wi&{u05*j~{2gzQajMO3E{8Cc?$T!#e*W z4ApueVc}#bo#+%81nnZ;2)nA!h|0>!PQaqI`1U?@iCy$5dwiYD^pp7bV{&qGA|C66 z$B!TXc@40Il#LA+J}H9AT(W|>jVTW1Nq{w5*4e(H{H;wR>q z_6(C*J{JBazzFLkk+OBQy{}EavaviRhc=qAiYr-170VES*|+|3bI9vSwr8YdT-w@j zg>(CNwkJ){yOkoS_B)vr2@N6V2e7EUbHbu0F_o@J7nt*YO_DDBo88wjt`X z?`HF(AmwwbZm|`0lb6K&!(&t{E92J}Mr-wpj9)f%iXTO26~1cG`yTEP z+_`fnR)cO9jz8MugrN60d~!!u3Imkh+grDyPeDxS7Mv zS`9m)q^HODOg`mfhkmJ@Zp@ok@{d#)LejJ@xRE%B6+HMI{-&lb?^Y^hG@U zHkmUld{XNVdNVZ?x5j5?2;&t}_x9ngp>lDw=X*rvy}RyTTr5ztJ9ojv#AK|vPcQJ% zBLW*++pZE@ZJW^=-Q_aqSLC=@OuqYku%<{(o;+D(Kgw9?y4*WBz4o)+Z8jxtY-|kU z>FK$?@PkUwZb(8}8n?u5ILy9g2OsbX{0$8a#rTEO@+9fk`sC&3`-g`UkGM^eiummJ zCq7QO6_@05uxl|=b(Mvc6@6@A&C14xjGJ3omyf)H!b1i2#&_2reXa5K)YR5ahH6{p zwwk0@DeAeI=se$>S5OcT7e}j}rTK1a_qPgz2|vBq(f)&~wNDrFii>3m&AvE{)nQ@q z$bQTWmhxcgpkTuhQL!svFbgBqCv68yda9SPaPXOhwU5}fIhhJcs^)vM14~NvAnXA}v!6~{duDL7IGU1W7KN}R?$;%I+byuZr*Qg5z)RC^qc@#eyh?XB9Q$oKEh zV(MVIe9hG0feXVbCdLSL;l*W3%C+CWDQIYdqgmx9nrZc=wX_(%ef##c+S7GvYHAF= z^AI4M&*6^w!ayEP_#+|E-=5-X2~xpC)COItK6pU1j|PL&&?j?ldf zLsg_`c=RIBku(Gm(_OY8Z;7A}tUWqB37xCI(3}i22Bj8IWh21JC(oY;_vv|(#<1T; ztt|S*ix9)c;LvM9@7XU^a*ztUE=k|GK?s$Mz}MH;Yj^eoosa`_j!t<%FzHE#Ve_fB z=o{xLcAF=l(cTd}Ls z)!u9E<2eT>r>4iJg`HH`Ukcji2nR<-k~%(kpp>g8I$Q#cc;aKsx$U)f9_Qaiu%pA_ zx=toXi3PlRbrO>ZooyQ~;zlYz6mY}ezrM^2m0!Xbp#K=I6ojp&W24WdRYZt|g;nv} z)6LCbw8oq6s^i(DkDM|zG;~{=x~cXIE{6p9L(1tH$51|gS7J|| zhsrt9Z?BO7X?|}=>nHg{H{S?PJx3?B{J}H}ET-ws1i8C+$=BA_B9?NyEHIcy$4@ux z{GN7Z(#X;2`ZDQ#_^a$2Iqti*+)4A&lf)AM0E1&=sW%q~AKBL)(3F>#(}{Vx#ap{= z>a@&pe7PJreZ5A3y?Y~`&$0XzK$-2O%H^@i$v^??-b9&j8tBkwS2I3L$M+B}!&@b) zXFAtbTf)1Hk5qfgZtcL0y$|OBHN+T}nA^%2R;pscW7wpXZSGYAqguuMmxz7*GE_N- zU%x5%wxls~kn+%5Da0F#u(QfFmXoP~e_f`Y)Y7VIbEmV+gXdQ1aCk@_qGW4z)gMk;Sw#iywPafa$zVs(loLHvPJg&t&ZEb~#AGvELH*;$kBYmVbKf~M zH8p2$KjLixG&R{8)%xqpD?=#NUCDBdy}cz(TRy)&(+?MYL>;*HaQA{%iA{RuG;C*& zMJT_K@M_wt721@GZR@|6b=hO;M@N}pB@&X6$wAG}vmX_E7##e3UPQGm3p@9D{^Ra7@$C-&gsqeBwrUi(2rAUY9$MM^6B84hMYYfJ zc!+wYYVz*>eE0k4@HCSz34_eXTLj0jpM1bl*L^y9=kZ~m4~{lg;Y(AN+5Rs1w34~+ zQ`Qsi)U)BQU!S75kS@~M`J>0@c%6-uJusJi^G?RsV%`h@a6=wHZmIU%3L~ik9gL8e zxN+e};kz!2ufQ`+%*-a%XS?$X3mXBr0~cr3b-@PNZR zOtn$yRJxFIb91Be_JYqi&!USZf=Qeas(P94kr=T3P$0GpBq8*dOh28_{Q?-6v&Su?LweYvfu z$g{=*R7oxnpLD$!-=-&n8X#nX?_nsAKl+7BzK34bqtE#-T^gUA4TU!o0#Pv2C00JD ztV{@q-80Larzi4xi1K+bI?wM_`J3qpa#axd0)2fB^j=Cm6MeX?s~Ntv9r=h#%JBj zrBmy3dU3F{)gOnzDLL=7uuIb)j5j_$9ugK70E`d-)-9c#cS58Lm+K~)!};t-L=;{N zQB1Z(HdZdzgu}K$wKG0G9<>L6RQ~Wz&-eZ=df0VN>jb#--sXZ-y+ch7U~K6IU9U;f5kgmSD-t6#XInpDo zZa|c9&1u~~Pdvw^!o~7E*q|Np`u&AvP$U;NC#X8g(Ya(yVpaX+b)|O0{AuIv@c+K= zd5t#&Rm3tC9cL2Lam-icHludKV!nL&1mGkYSny-h(~lxg3bKIE0~-5kK0 zJ3r$vetr7Br;Fld8Rc$lVnu4p62+ zZYI$SSBQ|ZtMPl*?oIC$n&A)I{~ZWFfRS_UFrQ9AcX$VMx4-N@Yus-ZQ8QXMECX1O zz=SKh`XUbKlTF*7S7AMD`)rZGZ5kg(wkQGZG3zLZuh%ua!DnY93M%s}lU zEp^e`I`kA2J!jEtCCqF|iRWJyD!g?m=Q~o%$7Eq)y50;sH1Ftg$rC+NQ1^ZgGHcRY z6wpfPlrF;UzLzma+ICsY3ndxmWGT(!nqQ5vrf3`sJt|I6P~1!iN+)J!nf+>kyB>At zo3=KR2cS{nFFFkeRnxK*D4L{*>xPA-U8{7Sr+LnGFW{Ww+ZPqjjBI}tG|zOeeI^BN z8!AOkBr4N`HO`k2aB;2M-_h>(G|X2_U}2e!`Dnr>_NnYF(40EZie=J2 z2pN6XQrFYdEA!mq9`TsZ<`lKR2kePdB~_vRXZtVjm@7!GeF6GKTSs@~f(N{{@w;lJ z_D@eQ@R7&g0FfW|%7!0|EkIrHa3(D6%>S5+2|qF6TYa_Ub-(o{K7Q-__^qVIbUghuy%bq*c9vcdddgVHgfmpVjo#qY)eP|3iSSok2 z=BaVEB*#lv%u7{R&HFijvS)>bFp7DF#GF%{0KhE;xTY&jiPdTL8y;ZJ)>}N)@2zQ} zVFSKQ0vwJMFKCdf+xN$*jEY!eVZ%y?(_A#Lwho7u&J0qhcBx$uu*}zQ-tay6rI6MW z^n0xu)NX3(Ahg$ATwK!hYsH{atJr)2)LRd`bZlXP1gbp=JwGl~m2cHK>X{S_3`FP7 zohvIVGgulbXA`5*)II_v+EwNL01-H5W=yI5Xs>mt&w&T1%ee9>loz#%>fjjH^?Y6A z8v*@5v&?f{)>~g3q%A2au^6qXwwn^+VMn2<~T(Nf&?CviK^|1aA02D#a$McL!fm7?FT(;1^N*Z&no(Ec`#{x zP)v{oCEmc;7)Mc25to2~Ram%E$N3c{ESA{#KVp~M(ahPvtCf|hJNFrIZoXtD{3_yY z^fc-z_S@%g-ke@oSU|ee)URJOKROy3u#wCKgg+02XsF4?a2Y&o9UUF>JT|Nr`txxf zkuk}FQUZi7xW8W$`7TfbQ4w-k9?32pEii5WnxiZHTG;hCx{ZKvC}?TTiyi|sLYD+? zgOJa@(4Ds;>BRRzSjYHLUrJ6! zKCGZX0`?&yuti0e9|i@D!=Zt&CTn7H7Mf!)7zjv~W9!u)+S97>x{73E)N7%40EwvR z)5z8ef`KNO!{ZepsG-c zf5<1Oj1m1SI8TqZtmaf~17$tz=^AFyo($)_{`*6%8|*1*)kA#`P^UDyZ4 zJ@YMv_&JaMW2DSMC&KuB{dyO`B|bJ1^9PwN-`z&E%@_uRKXM5b_ zuGq&<=e(+Zv?#@@tzrR;#rdjPFfG#1J)QtlJ6+;^u z0$^b%#e;vb(U22WiPOj(P0t|aCA2hB9S#+e8MGeI!qAn68DC>9BObpjL$xAhqckbBl2gQ*gV6zhJn&QfbAzkoxXijipmw8mp^dvA~SUS@A$ ztx#EIC61pYlgI_AKamUqQUC_|ToyFF4mSG0-sQ+YOcFUdY91cebv^w2Sz)N!vs86C zvurE+_3H>I_cMnHaC~Rr#26y+1?@+;To+U8DD+r31r|xIw9Jn#VYrK*y>5^Y2m|LT zr&6JCDeZHH4Xf3_)l3jJfL^se<4{5TLg583mhb*50Z7mfks$|*5DbwPkR|xs*G!-- z4D*4L??cKT-(|}XnL4!_zO4uRW(*X%+}gu|vEmBn`RBi~pYK$4+Ib#2OOng)*KFD_ zSq#$j5x~V2a$Pz_Nkw&keMSi?9V-h9HYi&U%GI5CH;=}4ICxUr^_#27B4q;wN4==( z>+A8S?P0&gTVG*AcQiEO%a4uwt_J-n3^#!rHVm|!WuiXt_`SN?6*=-2+zfCDG=b#9 zF$4pXoP;cckrM@G>|fr-H;X}FH50I*K{Jy9ytpBD>hx(O$O4C{HvTm|&3g4}dDAse z?(T0c+#8+-wWtxQhI4U|RSyxMfbWCT(4Z`OGSt#X=YN9)JaXGh>-qENNa;r=hEACS z7PJ{DFE6k1PL&hjLw?WIuWeY)ga!(ns-XTT25O*Gx@wrNOUeWx4+V95e}VCcv(@<6 zXGW4rX){h%R-D|LT~1Io9|F3r9~of;Wu_SjSj3s@!5GZT%a#HA`(B7Rf`-t7JTk*Z z{4Oh@|ItM;DOyCd5jG3-Kk3o}3Gj8m@(XtzaVKD8WPBlDO@l-tL}RkGilv;KEUW1QQopXbEQCPMoE)^TZ3g*b6)Jk%UbQE9 zGOLrV$ngVthfG@=A^u4y@N&?-pc6+xH?g?PPy`HjXqux#c)2PNgpqeQW7q+o(&m9c z0xtQ;%y|!`5il9ihl0>@CH2mmFld(lsG=9{_mDnl1S>)2o0#b0jvU^DH@Ymp&CJao zL5Vklf>(Bb5?_^*8e%1U&U31N_?{oFo*PzISJ(DTauhFc()GC>VnEu>@KWh?L-6*H zYzjoo(B3`@8UWOXHVZg-?Nj1DrxKSg_%Hl!=0%-7~z`(#IH`v!Ncnz_^AL0c~ z4C&mk<(2ZC`v(RlplUUP9Pt>|HkGKydL8)A?>xQ1$j}6T0{o-ku`vTTPfuecGIoDZ zEFT5PKg?%4a5;iOfQX)+UbD!80-uD=4}=6r4`72Zf&}zD?>(0yyWtABba#Mg0juUS z?>P1T!-vM!Rsw?KrzGGt^4<}!Y|e0S978gB=UdS;gxIiVl3X7cIiI1DuWSi@XliEG z(A7l|#VGpC+b}lvKOCprROC1Ta7e0*sr<)mvTlC{jz*V90{<+s}(+uMnuhk0a5yl8-Pg(!k@MzjI{n~ZISNm%jp zU}9W#x2it*{dmWMzn1#`b4h9Go+u0P>Vm?;RS+g7Ky$yLs3=&bvI;AQc3xFl^`Ct| z*4-Tvk~Tnjapg`tKOTsobiyuNL&L-Fmxz@FREJ*c`rM{cvJ~CBVW9F7ZVkJ_3xhfS zHj)q;x7xiW;ZxU~+6j$>7u%MSeel4!kvX!GNfJ?0)0_t z9|$}Lz8*Ovqu#sLDGH^YZVTM1qTRDvJlB%8?jGy-0cossa@%dQ<}hklMZrt-gPlBl z50-|m=HHQ#X6drPt^V)<>(6_AW7Qfvea$IN^ASrDXJWPF?UOE7&ErBs-pd!&RYJwh zKQ1F3s{rvKlj+&vL>I0i!GyQ6(kAS1r-Psr5Iy+y**`KeGQFMS(t}L>qN`O};4f^? zW#+DI^y#y*vB@knQYqnCPA|AeB1AOxaHOU{ghvfGMs%8g2&uEQ-s3TYFbAaR`K$^yYZ;%ZH2>Jfe zp?ChB>(bPTZwmJou2sHaxcVDkfaRFHqGIbS`uj~J!}P5%`2coAM~m{ z*std5aJND)QJJ02FDwjv^XAP=Yj(!&t?NY;BT(3l4|X^HaqhRLViZ?k*UMJl385y|1NA|HgV_NYIXR?er>SMq zpyK1b>jci4=2nHv`?az9K&0v+@dyx{iMhEkm|H7rYXM*ZfsA*oSSx$56ZQ?L6>iY* z4WXJL2l~rPf#zN>&@)h7_{Tj*>iXOM)c-q;0iC+v*lK>X)>j1^Z0PN!0nMr}^7T1(cB6@=Py}G$$M6?Q z5C#LsdmZYFfvqj6prBv`9bZ!aZulDXB?E{mIJmgTLIe3);cN|LVXlJ*#Kf7eXOKY& z;jfv2;tMDPF}3CyYG;uW2cL30aaP{KR0-+XrTEE5}^$i>Bw zVOaXnuU=`J2on<%XX#Zl#Bu930EVStVu}Lof36rdP(3s^SS-g+fvo~qknV~NosqFI z#mST5a0?9qeDwpu8u5W$a=p@J(E&C#B#M{{%{ygvbY6E|1G0qH0AMZ@LJ)FaI}31p zWg?u1g@=bsqJHgzzSk<=72AP_i24F^Zh=cIzktN!s-wd_l)(W^7z98rf>Bg!YZrdX z3&J7b6asXtXg7sYodaC|u78qSGYm4r8L1YAv=Ugpe!z8LlbwOB z1W-W+Is?dy_ZJ5Q(5V4g=gjvp9`V_;1@bom#|o}3i`ecAYaqn{f&qZrTfrVD0j0ER zf34lazxXU5ZPY_4DJc;@1X~La(nej?VnD!NUvnlBc3G$cBXR{Q5b`yf0Cl%-yybjl zp~3)b_7UK5L)fW^i2DVM_CkIt5^hI0+EqA_vR^$t27Vlo_}#k5Cg&W=ITk8U3WhEF z)veez=6=pwhh4S+mjTZj%zaM>;D>{72H*?h^_ebDrWZ*OlW+K<-UxN)NngaD_#oxMHdkt(302pWPeu37HL zhJs0R!}-Rb+;FWx$ZEbu+;*gj9DQb>dJo`4EDS`qgdY4NL4*lzF^&6BULNnT#STD? zF4!ykn}hZiKnVbYo{9Omo+|Y|H5KU~+kLf%c)$Qrg@Mj-9&RAeCL}u-*$&cRzLwoL z4!i=w$hVCRSk)ja`+M(g8iMhfa_tcml-?X&VvuK`_fDk5>7kOy3qT0PzXnU~TcP@< zw|k)l<+aUw#eO7$Ol&(*XE~EU^hyJ)bS)E8(|V{xw_jeC1b{rgxEKj?1m#i0Y871S z`oY1Iu%n?NG{gFoFCGN|YG7%}vHK=ix+n6F5YgwE_V5qoQX=JtBL<>Ei?9iF1lZAC zpukrvRX`dI{v=8U;zQK96&4%PQWq8)9(4hFnrq<34CAD}J+0*Q;K6kdkBX6hdbqnPC^d=v-8eQ1F<5N@b zGk?NvU)kE~8*CjNt!>jPetXbdKRkRIL$W(|5BLv~*-=mjF4Y9!Htwx%OTc*fCLbw* zpBaO*34}jD@|F2s27oIl2kYp$T`#PxtXw((efW3)b0SddiX(kcKp=tF z0P5MNKRbi93Ve1%eUNMqR=or{E-X%iIzKGaYaJbLgef4diLNdf5D)?6z(0N}*g@vuwReD|3)>6(nK{r&=GmxQ?TdDcS`KdBWdXs_&aX~H} zRuU4JhQ>xdaD}dljI?$trcEm{s-gCv5$&?X2Y{lnNc?Nm#_O9h}&$17ZTnyXzn!JZ2OP zTUl`cEHwu50{8E*;~-{A!um@U`}VdQu*BDh?TE?wOzz@@gko=9mWenq(~}4NDl6|oM3@t>QxvCf{RY z&S(&yfFRBd&>Zr|=YQ>6;NjyVV^L5Wzrnt0ZbtJECstl9 z_1JLdznxsWmF$wA#GUEA6+Q=~y5Epp@7a9N_LIjA9Ua{$C)*=OurUB7c z@Z})ndK^kj1N>)1$>~dGB!~(Dt!3utrvy5B~$Lj0+ zagZhq;SlkVs3<|gnds$d31HPHBssO zg+-kJ%#@{{DeV-7NY*b^@T2s%{Nz}dje!jusaE20vP!@_qoD{T^ zNKvkbwF#18v*&U(0S3`=auDW`@CFhuqVIWN&_Nx#^Nay5ejw~zF1;!`P;}7N4hV=) z#LEW*7Rdi9q#lf*b%VVi1+MT{$jE@}JM$P>V?ajgKz&BZq6@iI0bu)4Yd?Z92E_K! ze%^*l4LT^9*J?9uMIXd5!r;am{wOeok`N40Fx0LfH~ws|8YWOqqn-4R4+pr`5UOc^ zr7I7h)s^*G6{KlEyORUl+V165tQA>0>T?pd+Bp!25l&;6$|DosCr67608Cenx92G# z2aJ4*oA@L2DQvf%8$f|5doX<*%sVg{&7T|6Voang`-IZ5;lpL%)Esu#EvwUy0lE`I zO0vGsj!bSf>|Usvyr7jSyt+avQLp6)a{;ZOuZD|6_jul`u+5(PG5KnC__J9DcW&r^ zjn6=<3xo?9ySL|h^R1Y7*egN%IlI)dobWaH2 zAyY`{?WQ_)#)P#((W_p7jF43`B~*1_xbN$vkWZK$ZPsr_CXB=%DYm@6y#+Y z4s7^|1|3WF@uHs{JM3=PvcGMvIj|gBJlRu39pN#)jxsM}l~+_JgP!VMT&zqI>H_C;)iAU!;+n zf+9LSiE#IR@K=$Kyg#er(0Ri?guDH0k%}^OddU|>V2Ql(Apq;P$jMHy` z)oMFX)HOKT(?bow2Y)d1J+eQ6s@vV|2C6L?7xKBjFbeld3KGN{9v%Z1Onw8Bo!=Fw zGH-I8as$Tc+!H34W)czO94Mw|_zpQ$U5m>BBi-GY#6M6;htk<< z-)nCszxjrlkS>3m;Ox7{G=dyC~MS_L&o=uDtv0UdAvuRzvxc}f|;dt8q6B!d=bNU4k(68ijN>iUB#0IkU zCAs<-1~;NRGR_q5nw95-jbfUk0g#r zwdYWPq3NDH5TsCx$iG@CwsaiSR(6({k7G+qsO^KWjV7SziGq@p!6ciFbQ63$Hev)EK2QqLzy& zSYl>YPc{R0iv_|8P%RpuPCN#)1Qe9BmVY73U-Ak!1rHXdCrD`U_AryN@FNf<6a&e` z>VU2CLJRrxJ4iJ9@ZmmOCYGcyD%*z|>8f+jW490YmMQ4y2r+58l_hplaUl_qt@u&C z;~|)K`t=W-;w&zD7g!UVH$dkUud)G!g6WiDo9&q=7SH3><@ryAQ?1(oKg0Wxa$F~q=c-? z+YwSLolscwX^tS;`GT*4@<=GZ2yBO$5E}(E+&nxcz)MFldbkA{y+zYWAKDl?CVACAedZCHr_W zeweXh9-gtGgxx5jU#}r8k(6;};90Qm!MwiPWTfB<#E7NHaR~$bKw39|isc3wmF8=t z(LZ9^ryvj8DtAr{7bB8?cP40fVDl<0Pc)(k*Az0i#{1i=E5CmSO1;BiV5K568=xoU z{AE>A!T<~)fQKIx6P2c4RlMdbO|+)t`n{IVV#(2xe4$0~ZJOueh7awEPo2meLuNQ< zsU$o-*h!GQ5!_xZ9d-Emi5lY<6-D;)iY;d^M;Zgjv+>J#$-j%+w>{^rRN{>JsW1@* zGyY1q&rOu5)mLr)>)_a#=dM+~x?7V8{sC?yIC{G7YlG@*(&JD#{Lze_pOI->h`OBTr!ZV z^_+P<%s73zk>V}gaSTZ{x&sPDb(!j!U682_T!a?X)|u-r$gT9naN(mR=a17>^y{?0 z_B!QYW`ky36H^}a`W+~{ZMd-3j}EtL(XN4{*u3p1c;_Ibvi4erd=A_EXgTocCPu<` z&xT~a;N7_J_m`H zBLlR;9k@j7XU~Q>pJypFwIK=wwhmH!eIDrECaKbIzDb-_`}w8;b&=H8x?8H)f7i_j zj4y@NbCAHddzf%|qA9>ll$Jhw8uB@v<0tR|c3B#Zv3cNs;DzAB%_xZ2@S4E^tZZ%u z!(an&%KIQHvJ65kp#{dBrd`SnNDQU7AY7+vSgblst)Sl~=VDi**T0dYLi}f3hZnVO zC1*1ie^=PeU;)<$;1NRtJ^;KvBM_B=F$fGCf~W;3N&@Zy@U$*a%VQO_4g87gh(0$;+}vL3#fh8 zmW+g?2@t?7XPB=E1ltIb-<54wAj;MDlCHu*={c27QHP6nN_au)kO7nTs-_wQ-w-GGA|(kX`O{#(#4 z?q>ElJb2JOxb*8+Ft|yvb95Kfvm{(xuE;(Ut&oWZ$fX_P1^<=%SG)vQeI?p)%Ff2Pdeg{|Z5JqeL-%u=_6Di2bZ!1ix- z7^bX%#LtuulF(4bfAr{(mZy4M*);BEPl$`|uz4d28ZfbdnWY-|qKA+N13_gY2-Bea zADUIYKPe0Ho)Hv&3{fiUQ27G_APj7N0eQl9Lqb_v#YC`Kx+9Md_V?F;Q)#jss1P27 z&koz_Vj*!Aq&uWwZVym;G|(u}m6@+xp@EbDHU==&Z1%@J=;th;Par)VX}!an(4&k& z+fdULi^qEK;?HjW)wqDogHcEjI}MJV;O@ZhGgtWd^4ExytOEahQqpw1=@=X5g+Kc} zDT#MQxw!Suepfw(ub0s)gurNsJWO%*Q2yEEN|t=^lE%L;p&_YK&2}Kdfx90|jl-ix z{AX`S|Ab+5vnUlh_{}sHI=7Em_y4>^?F0@_JlmfSV#()SdHUmT1)-#ZZ&UyCZN8PK z_s0GdmJBYEDig=QZx{>XjtXIa->?8)a`W$Js!_us{__$wcuB^IzlW%1JeI=ww`!Si z!XeK7JqrbRi8Mtt9oF9%=6Z?sWo$jo0^0biy%n~9KM1{x^qIJis28+Xc`4vBW7;}Y zlD9rVr1H-xn-Gyyoeeu$-z8Jsp$U=qVoK(!FnrlpyE)qj|KRh zn}0tk6^8L-JjD8GY`TL7KeKFdKI5qjwWjF)=TKqR z+UYH6ndz|-Er6{vy&{apedn++TBV9o>+!yMd0P$g>CkjS(k|1vS*Im^V0*?r1~AT` z@3OdvutVkky0RA?Q>fs>C5+#eN6n)k7|gE&Xe=$w-dLx7{h8}}uVx+|RG7odK`^Zb zdCFD-(MJDgZkRfqT32RG4vLPyuWqWlAW2PM?JYJY#$*$oMDsmlfhyFsGWd+QbCq4? zm8lFoXQfJLMigqC_;;s&)TllQd!U0* zeZ#l%GW}CodIT0a2`d(Sy-R0Wo*JC0M$fz8H+k;w3L8Tc#+?yaLIzV)QzG&?N)%zn z*}~#Hw|-3o^2#8?_Tq}IR&39OG&~V`58USac#J(BxN$YyPdX$iIVHJ&Y*t}7%KEb(!SaHJEh0Xjz#GOA? zAs!op0!%Q3l?Si@V(H%#4*)NJ%`?0X=Y+vP!~t<@BS@dcGP*457rs0J7DoCX2AurTfEZLD z^ivST>&5<~-|_Bdz{kS?rP_S*d%3oQ!ZA8d`0UrQ`ZjyN6b~DMwe=-8YjLf;?*%Ow z@7glP&3*29Bp*w5k7G|Iqo_~CGWSBdk@&x*NNH6D=(xpik1o|P6E+`Oj%s=u4;Dke z4r>mlJ!vEEv3?$m)>~@H6J^*Xyf%AYc|8R*s%bZ4s0xM{##$#^4INx{NvR>SI7j&z z`%gQ$E3?aT?wnkce8`QA|JG1WXk>zwEIX*JrPa{f+&m+O6fp~nw58wR-vJ$A;pHWV z>D`o!4W(?kvJ>9NFo|v%buDUmgRvR-WswaW^kSpCb@_6$k2$D0)x>;6d`@Bc`^R2M zd0%liT@?oXu1iQrL@p8k^N^EWsXf|jEJ+hD)W+2Q^dA{$K6>sysfRaVY67r-(~KA( zi%TQ?0_N&Tqr6A{rx_?0*QA0MV3^cnQ#{0iPs5E&3t*d95b!YwXN9wB2|DJ zAyta_lZ6c9czz&voPnqlD5{Wsy$&pb9Gq9#NCqqnZ1X2CUuIY@18IY}5c*uizUTUK zqksu%d}OF{e$*o~Kb-A6zhZ@+R$-|yehr?$0vr3<|1}>E**>GZY!K_fdC_6hFx440 zbXGFIaoXgp$=HWwAE`KhU3sr>fQX@ForZbSo=64mKx2Ekv;sAUg~c|6howzq>D-jc zBJRm7ZK|cKqUWrD6cSk*8h|FRl>83v!oM(4_0?yz*N5pG3xBXbt%SNd4W_QUyBi*Q zLZRf2{4}Tm-%ZJ*t3pEGx0#Hz{_}}C)QiMG2$XJa&-0?wy2$_m z;lE#VZ@}U}0l(xZt{h8%aR0vR{j}1mYHrexC^PdyFzCPFu*Q}poX8DdTZb1Av+#@Jk0=k6KR5?^ zCK_*o=d7S^KnEkJK=dO^YGDi>MHhh}AdzypdW#VTQeZyoEoY97m+2yLHoW5*G~(R_ z89L64GDiqwB!hjb307kIz^$E^=dJZEZZ5t={vG&?kwDg&!L|U$osfh?8dUS>y&D@9 z=M~Y5DwtU=G2cyxIJ*qAo|hvf;Aap)rt8vx4|F3bfL|LW)jPjlNr58Fncb41+>6X; zatz_o{%=KUfS{O%@X6GJvY$Ykm+z((7=KBQD+$>rS_~nin`BDTIr?NQcau_e;1+EN zHE0uos0&dwSp$Q_kxdwM7zCkviEQ=8 zFJErkSAx?4e9hb`0T>0nEu>zf;HRou*|FIo-slkyz%-2@V}@jRts)D56bOX-tEN=& z>W8d}a_XHdZ?{8l+LA^xDemA#Fp zv+DC2B;~@}z1th=>?A|P7slJWAh9l=-92;zZczA3huh$!;LvjG)Il!wmd%BY`J^Im zC8``)AEb|0o=ZVRWR7bmmF0z9NGfYq1bU!(Q8nbQLCfdN?){|xwuUzq?7%XggNpv2 z@dD|3R7TO?1kg+YaseO!0S~P>Uy}VjH@sjyh)=nKJEYBt&h3_Dx@rTga**vk1}7&b z&@6ZI-uBK8eGzIpNJtKnQnD;oFM-v{2MiPpMA}M`LiQeODK6zQb{LI|pudDB2Ot*N zI5=3@{%<4=^+tn^XzJeSgws|l>Jr2@}6eJ$*LSlLxto{II!UlLg z%0uv}=LR50vK)||oz3gHX@g#f+B|&z85pM@RJE79Y`Pc@o)ZydF{qGkHV!#@^vk%I z2G|@58#L0LZrt_wNMJUNy@6X$X)Wm>!?iiw9% z>PrRtrvX~!YW9c?EDRdZuDBxb6YW6Qhs-S9F~=H?mL7w$#lrHGxnunQ>{HVIcTZm{ z{Qv4x)c*A}HS`r8jus2gMw`m;e_iEu{^3jE*1IX69x=bDAOiU?9U22?D5(AiJDoyt zR-xtM&95p>#r!UbGyck)3J(m+@PNmSsNQ^=_FX7$_v?4l7m=^-@rx*>--hSIQS?Z_ zgir>y`N$&aBduO|tjAf%V$KvzdBQ`bK=z`;;ZdT_Q~G3>ITd#j;3kKIJR+3;*~k9X z&;I}M^<@f7s?Ad1VwGDlpW|U7k~EuhRA2|MW{wiNGa(RAen=|r{!X~h-9!1nH%t&c zvrk$Y?!bgclJca^+;x>WL-ndfy#Di3soa4;5?dg3uJ-cM@@DY$i`MW%We{SH-IqHP z(=(KQ8h$ku9;z4HBFph8yTDA}h%+8z21+(d_rx=#@PXrjo(cCqef^)F*YV7%798bz z7zF33T9Numa|0b)Pw;=yFO*RRiQZ##ArvNDVTQA~{{TI02;k*A-=jk{RStCb)d&FB zf#rn;5ELNzi=yf<=>r){&{ubjwmd<~UfrF~ZG`7cgmIz*kTh49MRW5@d;4`5&j5q| zL#ZfWbePQGcC3X*OF_EtBnr@izWkK3Gv?K+@S!1{;c1w+#Sk%40#W&-R_Ri^w!{N) zy=J8L2U8Wd+wGajKjX&+;^0sMH1~%2{O?u*FlD`7Qu(hL{&Js#Yw&T8U=FPATY;0k zIf}kix#ibyLJkHJmL&^8;;h`Xx$GcmYORPuL((|7)=4V*a z`7`S>H};J+@g#Y7E!4rpMpJHJDak?=yyXqS(`nYrmsPji|2{<#*+2vYj~sill+(#+ zX&=Ez%lQ*wj;F5^Ij*hoZGCXVNIUDAYs)H-SH)U?OsG_W(;VAUFXu3q58=P3;<5 z1@Rnzhy&1)h||I}(BT6T5}2;o4TZqG?O&_i=^^NTV-T-`$uB&B8_A`$fFdC^Ls~)w zZ=m$W1axGWes61Qg8`%e!F(;E?3?=2Kfdu-))8YsC2NIPO=L|8OxYT3MgDPCA8yN_ zqfsh0QGf6UB8N2nc?{)$7El;YfIm;!<;NDgKakbzuiX-HUz3h#M88N8D*LQ%9;Ri$ z+DigjiJnmgK?BwnLxi_OI2E4NRl>uhh8vleMr)(s1@#c_MfK=ZE-djEd zc=_(!M8BlUnS`qc{vjHaM6nK#M(@HikpizkGFlQe`(~lj=%kg>Ln+;&;DG{YNN2F6 zz8(uOmeFf4Y@j54myT~lL3qeieKqiewG493xqqMPv$|K$o;`%HQy_Xk1d1It3E};D z&vI1@`9?^hVlV;BV*r3qcxFieb_v9Uk|FO#o%z3NI}>m!^F59qSwa-zQbt6_MP+N> z!!e{3$G$~USu0IS!elF%BqfzHic7Xe3`v8~Vo6hENvC9)(@e=y*B~e3et$xQb#fg!6xI zh~MvOFBLP8B4;Iq+=6lzXq+~~CNk7MV|U$@za8IZTXgsltp$+OXtsp(I?kz;i)3Sh z`R7oZ)k2j`Hd4~f^SDz>VRx*6cL>oLAdkY_IamPqxT_?k*O?Z_o!sCO?lAD{#WFRU z-B+;!`wK&qX_%KQEZ^%H-wVrilXt*@O-8Kd3$cb%pC8_6x-rD_6}$fK{dqHRUA53M zcM-#Kw&<(iy{$ zKHrbh(&>}mEkOs5+E9d}1W!#zrJ&wMT*R>^v{QxJQH*v$K^Ngr^kU)8r}iKJZhst& z#vz9RVo}~FeG37ATQPET;qX)YWUsG4s)j7fNEVq`Tgx&kU`izz4pgw;7C`j1)ep%8 zT-+WsWw`(O5nz8{!@j+I;l%nldPC&}x*zgFWBVX=GzsJi>Az3VW4Ec-nfm}*LZl{S zNYG&D5IC!^uaCxcupE4R6`4RIBcph%Lc+cPN^lzs?9Kfp&s7k!K{`XaTb4dHi>q3f za2Pl&Hw?oN>xtXRfx{J<-3W~Q+;O^ezi^np-s)L1I`7xk!bf(#av!`0*nzadPRTF_ zM&G?7b|L|Kc5+}HzY2B<`irCfD7|nj?~&eSMu;XdFu~&~2*W-=y|-{30QW5xM`!eJ z4`dEp^Liu#j7znhYe*EN2H|iOH&oaP3${yO{eS#$*QVxSvms-p)zV3CFPdf9S8)&v zmt7nJh5FvXnf#X)*W6Dj7SF6FwI2GJ&9B~mJ@H`->y!c)&5S)8z+W*o;+BcbgDu~# zKSg^$`dpGmnB5gyO0oiVaR@==(DAYTj0)0x(cXNzUic~CYrd7R3QDB2vpDU*MbP}o z{lh`(%n~al!GzBEK&PzLKGSop*r#>s&G&D=Qn<|DfLijFlL*}Q5S?a3FUBldr_DaD zy0BtMqD%d|A}fbr8-@jO>c^)((D($=2wlW2NteN2qTjuPV}UF{#4A(!{&E5FE6j5Ve1tNaUs)*SC5K_5QG#2uuFVQ zsS<70lTvugu=#C)>mAq;q1qPKd1 z18!*NtOxyQ)5i-%%LFda*B?MMr4mbJB0&&b5-0Z&GKCzY9Q^GaV6(sjr|74QBrsi; zFoN-H$(=z2E3(l&hoDsA!{5X&rI-V=&>>@m-@+E#jgaKX=x7x#QOdys0`Otlyz2ge z%bmS!S(}^3(hoIPvmQ*v1qB5b@O~DQ3iCg=hUHF2K?ZyXrEl&k^6e4u4!fgQrFkP# zSI4oP*h=01S=dD7hO@-0)eq|Hh2V|I-lLP2@hmSx=T-)r+cS24tg>g4+ZxX|Jw_=C zt#G1Yys6XxauCwZa0V0FjhKugoka^VrE32%Tt&fm!FIe290wJM4GG%WwW@OF10jQ| zYw_byB>dn|Pb_KQfkCj1=lCmaJ4sKSoRN`0``WMkHg9bj>@m#A7#6&_`dCg)0MhMh z?o8Gkn^fU;Sl-_Q7k392K$u|kd>`uu^LmRRcDiQ8Szg|#RKwj9(eQL|a8M@V?{I`u ztxH@uK@ka!YrUxS04>Potz58RI-KN~L`6nK7-jW@;Bye?6)6Tx_v->AG;3<_RX@+Pk+~SbN>vC8$ zcH?38%dv{QY28|TT&_+#nz<0iFshKipPTJEU%z(l+#2=A66xnV`k$6WU}`9{*5I)c znQIZw#{zr#`4K-~-;SStvhNI{vsGr$bml-g7LogBA`D&cCJo>d$xn=UCYZ94$n+3# zg0a@oF{wSNl-%_Q_rAg5+70tdi=XnE(3O}It2(;S8Qgl+(a=Xp3dG%lfUg+R`%dco z*|YZVChCnMX5GY~ne~GB$M~8<9LxXb_xp`a!lrz{_dSzuW@6_aF+6eZ#+*~NwN~eE z2+@(WN}^OXFqNGaV!e{FzJ5%zy@^X_;)VqP5@%iMAbBMmLT{mzCt zy&{+D+85zqUE=G{M#O(KI{xqef1WuY_?G5_b2dpvsXoUw(t9F=LWp7aei)&$9yD=yGo=&vT7@2=I8W6+(`A8y9RH*2X$-20UOTVdr zJ|@)^p0%EKZR)t*y^8O4K3k4tEw~0VVL!t4if}k}?WZ8)hr&uhaj|b^s#zHC zV1M_q))~&?*M&HqPWTqNVDpaMU-pk_<&bV49rGVtH0WCi3`xCsLV_&CreV6ii_?`> zSqnWk$lWz#3+#3s>`Qibp5=dzj84%t5@%gHV&Tk(+kz4&6qDD}(<6`ziul-T&k^C8 zx!5-vFhlwkQEreQ0J!q4*%d2`DIz z3T@45jFRFn;^R*URJR{bU!Gy@$-7z@qPgwYyAVv`_xSfX%8SaBk7n+L`nNi8fB|kH z?D#>PXHTE*?r&d}#v_^2lZWU%?Ar_!i6T4#i~a7ZkZlXi+1GLpC#9Sxmi;>Eo#Zl5 zy_=Y4CHZ$SxRlf`kEWlerUCY#?c29XxcZqCCPkaxz-xM=sw#VcKL4>To7Bt~5ZTb` z@`n=Gnv_I?UCPX+JIyg}x8^AOXzvY)*K9SDK#ZZKq@>*TMwY1~yFH;D!Pqu7$|c01 zj+*)E_Q3NXntIrQ6rNRo($Hsp z$tW2N2!pTbZisD|0&b=bb86PtD zVDAv36?20Kly(%oW6;36SUnHH16)S*FlMJYHw7DQ^EStl4`5vxX_3jZ15U)B2&293 zBC;>5;SsrAqU&SZgf}zDHGs>>&cPyt8WntdfE2~m-LBR~&RqRWWqz@mb(_}Gp?$~~ zB~BW%lH1o07+%A z|G6^eJEt8o98N+9lxE*l$5a>PIU28cKkvE#%cSy$Cr2T9*~Env%g$Q)Z!%D%I~Il@ zdxzgze2~;YPNepaT5M1oNhTZ}?vKO%wKJ~WZj{~1Ixz8haN0 zbOzo9;fHzoqzpC=8Jl0n`dCQ9?5{}7$_*e_D4c7^w#)mvVF+Xx1je!o9p^e77_B;s z^0gC}MfWp1TA;*68=JM>hAG(}Tx@`?5P3^`e2slm8%y}E=}VHfwsg!FM#=-L94x_? zesL4om)O2;#E~BULXjJI3uVQP)0-XJuv+h;`-9-?^7?Ikl69U~zA4Gc0?6r9V0MG- z`?OuV`elaWUj!MLNinX};zZZyG^>GzytKG5TYM z=)s}add*H!#d1(M@pd-w6-W@S5i-bhXk|I@ASvF)K!=Gtj)>zpHbm9l zr-sX9E3Hdt+F6_BSspomFBvp&KHBLJ);d0qBKqiOSlEhpJ~fIzM41d_AtPCbU#VbWsnv08D>Q z(ruZ{h=gF!^Q0+(j=jkatKx#U=Gi?h<5=PMWO)*S2@BpKmjM`3(Rqc z5rM9(trf$RgTU7mfyWF>5K{9b1`$(Xtf&If$>L^c2v*%Ro$?V?kR~WHD(BD&Nr|8e zX?eW>%wxF{A)Fk9G;F09nBAN2G1SN=!le2SU_jwegEJc5Rzxh2GoFPAyuhC`1$G1E z4LH9Nlh;+77BKSQf%%CVbdvV z@jlFt4m)eN`V9M;hPKm2^QbwY>N<=X9wRUkl8{`QMPdlJe>OH&V% zy8UVA?CHFZxjXJ0g%Rqv);yYH;Q~NNl^DMrn1E6S1HHUHd6NkTC$F=ag|Y7)jbywt zYuW)XxZEh@)M<}~CnvkaTZz?W!%a`)N~m>5D8G%79Z|8ny1VJ_1ZiLF#VGa|rGs(( zH=ea0ix?(&3bn%hm+!|}jQfvVgwHIs$s85QVps$8^MPN$pan52T5~8R<$50@FGsssk{nWZ+wKS9n@oDi`-^Cd^}w}d|aKZulU;C^LFxZzj;IG20!-|M;{+g z?_0dQZvXuYH$3h+@Qz#k{s1Sz^HetSMxltUkpD1>qzj!;s3#68@-lkp7wg~sO!Y=j zaJC2RXoRu!DC8Anbf-nWeEd$K{CS#lid&7RT2S7**Y+m+15=&qFYmIB`w9f_D3DI8 zB?zR~n6^{M-ocQu%n)ml?z(*W^1k?`FM%`V)V!JEOEV{5b_66|_-%@%kjRF{z*mp# zM20wu1^LpoZda3sFRBDQUQ+ln61>`kd~Z9Hu}ucwCo9|ZeZIcY#Ss>!Ut+FOR7b1L{px$Z{rk7T0~&dGJ?CmK^z(@#&g zZ;6Sub#`{H24^mI1`k?GOSCr+R{aY0e7hPkSg$3j=hgbOUw8L(d!llHZ|x_y{C+(5 zXaKL;U`z=$xj9QNzOwg$v|dAM{B+9=%v#E&Qe)gOoQC%NB^m=4TvOV@g*CG{tv!Fv zXSTv^4md0%$0O=+ijTXDBX|6RS*j!`m&~5V^n8l!j+A4*zU}qrf#-uSYmS*OBfoBm z^!O8DTv6lv9#i=^bZDl0Atf)mg#Yr`g^HCh1&4P@&SuY|dd}2+V~c!{mv)x~UnLA~ zxkl0pOVi##rc_UM#hBsEgp+b#GmgQDv*&$Jp>ZWt59cqFhR2}~-tKj<7d*$y72!!J z<#}Sp!YPEz07~S}FqpMY|7EZRUt7sx?uq*df5DV(_`WQPw@ftgkBZYG zt%J8Fb)fhp)9V|tr|a9Uf&OT0S9X)XJOX^I!PrmCSvV=-lzLWtG zG^h{>5#N`~=V zf2XzPV59X$?|9=qn*q<)U-$_zoY((-?%{Xuiq8E+q7LY`^be05B1zEMI?{ieefDaS zlKNLy^$UHEXf*9>+RDqGka{hXUvm@Ezxnp^%+03OrIG^T7-e&HbZk-kAbV2-5s`i3 z$NDO^daP82pA0WYM@tp==ar7+*1Au54*keHIZ) zE3-ad1~=>ry%4!63kz3UM0yP8#pKK!2Kz`SMfCl=28D#50&VOthmkYM4(1nPqS%x; za1|yD47uDJFp;A^cmH$LcS0APyGuGudG&eqt$Ffey@_}?NRtcxd36unRRww{3VxnY zRbLg^a=f8*FE*wG2bR`b5<#cQ6zu13JJrx}H=ljJhsDBz2@_o#E5}b%PEMP(Ng?6a zJ&N?07{>ox1=mAHsHyXgkI@r&m?Y%v_SB5jiSE?AIasLY-HK?|1^X-F23O`x%L8L? zD0#=m6p6wKlP9jTVclRqdq`D;)jNSDi^>#h$Ov&HYM$J}7aziH&9hCZPt;w97Bvv>|r1*_%*{sfZK*<0*ORm68lXjC2{`bEY zX?K_wa=g`&*QY{duSTqRI?qgg8OqSNs^+N|wVNyXb$?tHzepaN>P3C@isb9(*L7G} zswiL>9KK|P4GwCRm^D&qXlVSx4=<7>nV3MKdS}|cBxh)7k31Un?3aJ&e68I-BG=4V zoUNV>S$e39UAXPf0hQiXFdeX&CP}Bmm22;o$O;4kUs2I%E|)$#%q!uIE&3{-=o>LcMsCi0QJ~6uXMLX4k+gQ~%7okI%0q8b z88qvx0b7x|tZg~2`r0i*6kq9@AQW7As2BDh2lxyQp~qo-5pEavoV{(7Snc=6{vr;J z%|w;m@^Aq`^Wi+f?#2}L(mG2-H^wxZ5oTrzc;17pV<9D{P(M94fd zdh&|wLYP|c`V@2EV#?}U{0y9_sy{SDs*0&r?Cdl;vgCCfbBnZ?(jyjzTrt zuja5AlNK+*1k|_B+Wxb^^pO<`}5>LR;~o2LP|{?z4^V7?mGu3CqCeiquQ;hy7Dg~ z)14nbK6MIFv+6swd>~D@fpVn4Uza9v+4NLZXASjppr}uF5_L1=!3j$;c2Of;cim(; zYi%ZD7<*P8+XMi?ZYBE5moK?=0BoK_pbw3l#prJNT)Sb>9KW|VmSTYH-%4V?e*LP`;y|vwXkSY@BN?!7d46_!Y)g%LCgPDe zAbn0B78cfdOR?{nP>$BM-wD57=jZ>V+v8C$)OcB8)e(i6RaaLR{UP~zEL=Y`gtXV| zlA7h{Y4*3^rt;dm`pdz5O{5$0&9r7&&+cw_jF(mMP-rKg@9`0kXAwKbn*Yp8X1%HziN7` zURMeQs04-KH)yS4@VMBn-``W3)VZggE09rk(t-i3AJ6xsphOIVBr zYU$Rkp%~ly`%&rX)UbrCtyy5Y-gWeATZkfJcqIN{^NN4~nTm>v<3ts^@77ELHX$tv zm6DS3I3uITBMa79EI9{qGoC&^@^Mr=6;Oe9*TyvW$CUNx-{1Ylsnc#U7*jTFM*vuQ zvCS2)9Ig|5v`GE&9bH0OaQ}5SyDl&fhGJlU6x-m_!YBEs_hfn7F3>GwHnS> zDZtC@VdGR)k@HgYF{-^SFQ1w`5r=&N6O)95#B!4_=8K}O6S?`j%SvdmTzIau5>xV- z&N%9Ymk(yb!ox36@tT`DPI)bKrRwU*|FSEiHBW?=k^Ju3Sj2<5_DJ-`yRq5X+0aM= zYN~OK6t=5PwnAUDhg(U_pt4_Ge70@=!GCo5K>nM$!La2j>3+ zX$FUw(Qlz9Ph$41xQwErbPD}_)z(k3&t0gsOju^72XgS@dbN*Un z6%`ZnMb1_lb;U#Ff|5@x^!n=u5q181tB=|6QrL1}%&=QLN1cT>f3%cauqX7;N*JSa zU8NIC{+V}W)8sSD%Z|9r+xt@(}u%H#dv)OP>u{biIE* z@zwdhUzDP)aFR1RPNdz$@TfiPG2=gn_^UTuZK|xSJibgr#3*6-s_^RDibuK~pXr8y z#i=6aW0dr#IepdhOw!n??=XqgLm8fqldhUaqS9f2kzVYc{Q2RZ0coYFTDP6m;_`VY zslV{!il$c-(FxycUNl{jDEKA~WanhpNXwe=Lq+0Vor#ODRTXiX63UnNy*I2jCA+lvm*!BnWoDq z&y`{Juf+xfrdHZw!?x2(t4eu8q8vB}HV>YO(^oHJ&-MY}O3B0VBtzBM5pr8xVPQ#0 z`cL_Q1v_44N07#Ak@)`p8YU|E?~anhpD#CIct0io+6=j^K_r+$xc;oJ>TSLT?kolc zn($?E3@7%0mpmX~&u-rm=y58$Tb8|C^?CfxX8nE$pKo^5W+N|D@ii`3s?HO1}>r`pW-#q(2mcJDt znfYU*z9B%^_+YE6rfej;Li-E94wO&Y`iKPS@2vn_mipE=h69o9Y91Nb^Q z@JrhEhE&yp_E3xq6^=tJ-jNE6$|5|OirNi|=s$_eQguIZ%Vzrd(6100 zpxtR*8&L;s=Nn^Lp$bBe04%HkCP2zq7i zRsV#0w1SF`taA($02W10Pd>#&dZ+%1q8AeHol33K1u67vQJ!{!$o=)h{Q-2W^qiB8 zXAq}4J3B*#E_5f^{5{;M_uZhCI^EU%=QZ0(HaV4I_77`KS7H5A?d}G`lud`}&fD^r zti0*>qu?pHViGKK8-PFR=+fH?FQV%zoH`AWW~CK$dCzJ3Q3AxF1R0~HE0Y8J{0dLcoCgp4fL;{dIrp4LYHXPZjDD!ap}6E5%% z^saUo$c2MYp`X)vFQA-9-$ue9&#g~XAN#6{a;FCu2$3gk-xGvo+4DY=XHF8mo!y{u zhw@{dQ}0hrbmmVe^~oBY4U(@)65Wjj#8hd0@XGd29zHP*Wk=fkwcT- zmdNkO8>^$=OalweC5o`2xOD-oH;hOqvcyr={&kwknNLh?9~O5!147Nhd5PSd^T9}f zdUTLI(PI)EjM=b!d9*1-ZlYq&%ike})C&>!`2z^1vsi($0%t`|{F6r$UB2fyT_yJD zL6b#?1xG6CuNITZwuS!LX~rx(rnV8^9Q(xL9ako|n>w!sT^|meW+FdNegXHMDe-EW zs%}%boPGsAWrG8hpe#$(x?Xip=*G(WaDHVFN#0GVy_#NC^cZt}6(4Lu=2`xp^^9?s zDER)%_!zmY+fK!8perq20#+1L@2n%S$8>{t^3$ggR6NF4Zr!5&{{B8!MNnX% zgsiNr=jOB+q7Oc~dZ&GPDDN|mc@C9wf@b9J$7FBr>dbxnOqkLh$odS6y=HPr_0?C* zqt9eM^r`rP{>!@bfA?H2k@9GdzQZ76w=^De(i*=;LQ9R0QujyhS2$H_?t7Ep&4=OP z2-yVojAaqHgLS;W5eN7`FZlHMXwu??KP9lgjw0gMcz#Ks^(3iGulWHPhsGRTJ&u))?u{`{F{x-2OVLHmxfGK7RY#yT#c$Z?g5MLNcdPNSwo3r2Z0+gue6^aCJ5t82hzl zgcsUQmy#gN!MI<{bJkPAuOgA;VyqCp=jN@gHM#2OO*b6&QZXw)>(QS@(?he9gqJ~c zgzy$(J{9S3G}U?hh%zpXijIzM^Cc!GX5-)h)+Z)5;dnAVZ93f?MEB~|tE2ti{(ePY z-`X=T<7d7f^$K)k3UnyBzI{E^vpvs~jA~z?jUsZgYnt1!W+;&_yH{0Qiq4@dv?PId%t^QGxt)(4P3z!6M-n5I4h~dmreYyRu#E2X} zlI4e^yQ->cv$p9=K#+899xg5fA|fJrC+aV6A$aAu+sVlZTCl98EkJtPA z`xSKq7A|k~IO9&S*J0J|@2w{XvC^eHwmd)Na3!;ZX}@<||NQ9B?$lXPLie?Gbz$$V zjy_p5ym8}({^@6Rn4bTA7^jQ0r)_%A`yEX{89>n-L2HHC*^|pZCqy0)5D*a=dHLn$ zmuD4B@TQY0{(0Z)I4mg_JdA-60Hf+6MF83VI1tbd@(-ZQ!WTC!xjDH}t?T8$z(62| zC~0YFda1zr!F>=VxBvVJ2TDmR>>5MKrTIIm z5Gar_Q%;H4YCb9J3F6*)cFjrJaCMIa786HSAQ=EJ&j$~>K{fT-neSvdsIGCz6+E?k zGj@?QlL~HN{NIwy?=~sO#>SQngnoIdj+cy_JfXJ5e~%NoeKZI$ur4r|LjLjt|Bfy- zsY~OwY%Qy=8#?5bXhb>Y1<`=WF#$ z>zdPil_&BeukR4nemL1|%!S!|HLkMhX$>zBv}>psK*4EpKEE(-yZ&hYFescBcxRcS zkf~(OpF}q1=~D`f+aJ5TsmHr$%VOtu!{3>b%d_z&ZhzwD;J_)=%u=+p%x>F?y}iAPQ%k>o&4ZD4 zHE#5)>IgLyTSLHLweH^^xlPr!jg3)%2sr4LDrybv09eilrQ$KE z<*s-MSLxiqajCx=qa0lOi>OscCB=5z5I5PwM0+E_tvp_Jm>T%R_TJv8^!~v?H)v=4 z9y1a?TQlGDz4QfPkSNlHOo^ESw=w!(h=qeXN-^;*r95_iKU@eIB54?+68pYqTGuF! z6!~;@bbcrFFMlmFn{o^=F)^ucXvpN&*X6onK{N73m*elzQr5j_gB+t(A!Z!4_l#YM z3~>zNzO%s0iww%mbKH22o?TOsrm;~$CLK1pdFI5#MBBgsG1N?KMAI5<@OH>MIXpR8 za%u7SA?33?h@o0AY+L!v*y*@EBO>A=+TZ^ZgL$O=o3RLbZsg6*h7(J**1AoR(9sbv zF=eqFRDS$tTF0})-P>B*H=6Z7bepVE0iUwcd6d)Z?5`gwJw5kZy*Dhttgc3oSc76~ z4XW4izCoEODJbdIP=V$F{#%0}Ec-_95e#$28hg*{{2&ClR|cKMrQ(hUWm6a71W@Z^!JGdz8wue~AH-AqLe#qjkHC$DlBu?{}Y&w_;XdfND3leQ? zem>i&fxZ3f<^&jLnD}ZRVBXhcY(MK+$_lwbE9iIyG!prf{SCs`6MldCL==)3B`-~O z!;!ullX$?<=QcKY0K1#_lRno_Ikw(kpUBnDQ*3W*lhG?TH-iascYX5R=S$k3)kIQ< z7c4xQk6x3s`LJdFozx$t;XJclZ*#imzkU7k-1A!_3N8RcFeSiD^ z<{4c37;f9;4>@}N`E^HBTpYA$?`S!B`AAUSB_$c|uYL_nAnf*wDIo>-yx#B6)uyH< z#7!qnZG}asU2ZPv9W5ho-x^Uz@#5u6496c5W~Qd9_b;WYMgn7E=Hnw#Ezs%T=A?Al zj4tZXu1I#}Ygqdkc*=Lu9?p=tEVG5A~6`ODr*krx`n?@o>XD)d9hIh0LmF9(IJc}mh5t=c2SB%3Psak8Z zOP;Iiq==3EUgo+|!9!L(n0N)P5$RtPpS;2@(j!T^*r>n3JYi@dP}qCdM(f*?dUp%f zAKpt(h6h<(%g?;P1i#w)U&koDEIw6o!b#t5(7#)F?H?s);D%KE+5>@iQ=a5w3|$C) zn*BPlDVp`vzCzOqkMD^^?jyNUeYpaiOz}%Ban=qF2Ctp4GJ!}IjiOM@Mq^DG|C&KG zY>+=T^pCcpQOOS?32iVwOo`dA6J!97!h810ZIiDzr&4!I&AZC16mxk<#q!EyG5v{$}v)<8#xY!wn4 zZ?kwnO_m~7LR(?6HoYf{T1ulI;%K~~VMH^&NE|=)hxi`GxVbB0#?~Jq7-$Pv&r)P=NE!O9mZt^2|k&`fxi-t`eH{p3|=e3~7Em?F&M%N+Rkts&T@;RT& z4m1g%s`p4~KWn1l1cC=XZNxT>?Z2$9M<>K_p>AiU-hJy$A$Ux{hjEqD<8K4AKtk1- zEB_q(pUTcA^d(GT zn*+UwN7q#WkC+${d++!Yz0DAXp|S}BUE;bG`bAlOn;zcQf~o-KmtmaT-|Sy84~Txq zY3LBGOKfPBng}W5kDSI6GD*=NY<}kw2#k?dKR zgx9=0Xh*g3C+L6xR#8!!wzjs{c#l3|k(&!-MUSKywR01)BiBr5&d>1?YO=HA#_0c| zUA>=P^y2%ZFF{DqA$8Sx?>CR?a)%@iD#)ZoXO97f+ z9|Lx#%;JDx(J&dYfpPIVJ0Uxd724DbJjCgeQmw1f_PypKOZ&J%VPy5VJ> z$ymhxGq*xhu+8w~W|9Ygy}BVvGMv{+7S-JwH1f>!lg`QKiy1|S5yrFS#OIjtr)Y`k zeS>WalGWcwM@EglJb)q|BI=~~uP-v-%>bf}uRR3}gnR`oJRr zu*Qs+`H2}NVn9h-06=93*9RUOF68nJg@Wrufg!{EuW_cVs>-R|r01n~m=b08In4?# z0zZ-kuZ)C&Apt=AWj8wTc)?~`oC=<6#}ag#)H_G7eJjn73c7{(v5E<_R=>Vv!fih8 zHNQZaO{S@)p9Z6{;$i%`e|VXFg36F*K_tX5X*q#tYwJ< zKSu0IaC_(Kegt=bO0_+##yeM-6)dp#Ij1{D_5F4P2_|oesQD@~a?WUW6w5;M^xU)f z7p$$c@5pj+GqhwA%i$uh+$oy;N=?Z$A_fqcJx*6!hPzj^gS5{)5K7A^{@CVc= z^xmk!P=%EoSYFF&%#V(P&P2gJ3ryk#^WnC5{o>+cfu=7;w%?y$h`rUEz|Uh`6Dqs2 zEUB0{rbek5$v9_LVTqv_PZO5Rs)UKk&C9ENuw~9|(M+$ct&O$0w7HoG`t0SP8z6vT zU|?k2y3dKST>bh6B!49zA31!u0*|K`WDN&F|(l`$?IIk}+mC7rExT>2{=!4P=CNDR~MtK0UA=Gd5c-JJKX z1f}p?UvF>Q%1GfJli4zeW%J{eHj$fCWoF{Gc6K>128TbNXkPk6pU5Pg2+N(Wh8Q1T z*2l*utot(}X6@%CJx9_PHITk&uK1AebCT*CqW1V?J3(VyIrTMDjYtv0sI(M+1CFy^ zWw`On;?F8N{(mH#DKtZQ{>#!F<2vU>2IJ)4Hx7&+ zgZVw%9!YRGAIH1P9;=i{e|_{wj?J^FbU1e6&Hn`(kKO z(KYCzKc|{Kel!b|KQ|vtKz+;p#^f+af?|7)|ASZ-B}Qwxc^S*y&PfS>^%mZX7-^K4 z`=p!oR>UewP(*cq$K3A8gFt7VepWRdS>;y8tue3Oq3xw%4Z-$JIl`g&8Tavt+iAgN z@tns4ihkGCq(M0PL=*d$EiwXWMIfaH7*0JsP}z{yl$W*|K0>Q~hFnf!V&ZtU12Nd3 zI5;@bfx0Po%gq@e0fSIw;Nq}4Gd%c;>k}CzABlPYvYc%VLsAm3CgaQ+eKAnSyDQ;f zlq|d(Gna#De9zmP-~I~?2n!cL4!gGJvb(>JQ0(pBpLsiGW*A{S`!(lE%p&g~q9jdj zMbg#4prE<6wc?QVt*s>R4Q-&W97ojUcVDcrBmQ4x!rx=psiIX2WYyI#L3#Qe^un5_ zio)5JegcPiLT@>E(D5Sn4e2m~-j!pw8F!6w@iMhHpJ^D=N8S-=xKD=ywQJY6lH7ay zKI@g6>}>xeZ%%{Xg*@8RFlqXBE3Vaf{j>-b>8V^JH;2FjbB>AQMuFLYh&1}s>=V5* zmSUlLDq~kCt3bw?RwT4Y-eil|%Qd-(ahcr1IP{g@ugpN*^i)0Q zcr!rCaaC@DSB00AKy?iG4#H|VDu%{3AsZupGs0i7d9+BVAAj9DiQOgJI zyFH4KZa2vVn*F>DxPvh8Qa^kPbc}vk$ls&StNP@Z(TGd;rhOkwYLfaQTdu;?M{!&_ z9|C8;R;op=NB`bldf%{3!FkE$7Re1p#tg3;7tRX|Fi@t-8r?V55>p-Di>PU9hX{W% zF*?qv+O;Tcd#B}nyYHTv(;bo1wEl%&&d|W(3dvX73G`w*A?0^ux;I7&iJ{q8%z8C_ z_z;(>{Jy^460%)bQlKLS1)iTCU`a7CGP?7XZ!dUZ_882MBWec(8Wg$|DW9)DAUHZY z`Um8gH3~!XOf8>zU5`W@mX^}DY)U}j^?@Ed-mRjn99mddcvjic64Gq538^aiZnjLl zStIa4dwTkX46lwC+Ec!W!)-6~65wczfY#u7eGkfT5BWfcBBn;5Ac@Dtzxb&>7f_yi zzSyr!?nUHr*$%<_S(PmASfC`<+cNqfo5oLaSFtK-8=i*=kVzY{^xsf`lX>83pyy*b zkQ{~$2p;UTF9{)D0F4WzW*pF5Y!*MIA_WGzD~6*Qq5E`w*!{I{7=vebe{F0eAQllw zWpu)>NW7r({+j+Nzx3a2IZ%=qz~$=#pLHA54ja%R3vOf=3KV5m3pxs74)BOs^W}xQ zPx4=V|2l`t`S7djrTiaJJ)<+0Y${|d0h-~Z*4VZ5_vDOUU+Cv3nA1=sntnU()rn*& zgrC^b;zwd$z`oS_;73q2orwGFsDVW%Sg3Q0i;-a$a9D(eYYn?E!#aj;Mtb>jEa0Gd zc$)DK0V1HFpilsN1Ts2!;l9DJlm~<}RB9pu(Tt1Wf42jO+H<@0U^D*Cb73n`e-R*t zASy@)Exajwl~PAuq)h3>XfCF&k(YvaI9-gyTe-pOiN}MlM!W)lqfQryvNpScDcsJR zQ|3(;Y5)@#OfA68cXh6QaM}PNrwhtfrPqRDePg4|;r1K?!(f1RW&*Yl4@rU12C@t^ zH|^txmKe)FWC}Ww0ld-s$!~$4!)n@YC+PGiw3U7DWX{=&TmFKpP9 zgU2C`bxW`)QI~j0S}N;l)C|G=Q8X?jW8&8Ti2{Brw@8Awz5r=8dAMYy)5z6}SXe7h zqSDR%@h;#HaqG9MQK{F}RAfF;N#oNasU`N{)_H>x#|bZFQ)#cuJ0(23*5EH5t^#`# z;GA+UFhU)Gz#~O|1d^Zs{X<;qI`Poqm0y=-LSY=LkzU8qz6|9KJnMVt@KX>`- zry>ivB6{2vjI3vq6LdZ*9*Vv>h+3q;Lb?14;xf%d7r=H|a=8B{lXV zL;zuniHRN6I@sCGZO?Vw%Sz$vhTFV7}(torI_ z9dnUVFRG|dbJ~O+I}lqKP8I!m8uII_oqQzMD)A}2iIL^uMg^l9$K;Z%3PV-QAmA8! zZ*&UAHz2Y*SZu&M{ULxFsN796OMO|$R={i|ky@H868eEKy1JQ%7bq|!Pe4!1W&&%i)9S**u-jVYK+m&U-r;OOB~ZSXvyG=A|t zYw{yam7Wkl@_nsSQ*^Wm*wB%0hhr1&T7VL+?t5MO6u11ZN(>3+<%tID-ZITTPEl&o z2(yKZb$#X9NelVT2B*?1Sa;06vAFUYIOO5qVZIJX`VmykOSJtW9AmHkOk zq>7G~`fW&wnlEdBA}zxa5AhQY%`OnqEB*JJ;VxOZxCkJOk|E(I0BR(X+C)4_82gX# zOm@38S%JUt5FQJZKM$aZsLwJ9l2-cq_AY^>|86T7k%J!s9_+49kV7!e6RO5go>DRx zNJttKFtak$0hrzxD4=z>?d`7uOu6i{`W25u^OZa#qal?OODuKRntf>j$Tb`;0|+gG z1gH`%IB^I!KlMjJxL8J37PI+ue@gabJ}*hCJy#)4tI^jj@b6AH8b$$8{=rFjAr(Z+ z`(c-?y`zKQXITp@qf3xZ2z&ngxseQelejN8cm&}dEvMfc+n<9!2dRfE!ovT?kN}U(_aw6-#A7f|i8v6R)${)SPKb{0 zvXkF=COE{@F*HO9Ft5n0u{L-L{PLaIwhJXDbs84bjo=#ZZv9{a6aYyA98|VyD(4|2 z9xth0e)#ZV1-$*B_D1+Wg1O;!bJy^;x3@!*Hd{Ib;^`G&s36LXA&^suAtwN*TU(G0 zIg|L+|I);DK{Y$;Z{W~r2jUwZBoXDm(u+H;NvZO;8jf{-I=)Vo+ zbCIO{C1 zBsf#?e+2#^1kAj_GVn%i9Ao2Jb$K~2L<+N$3f0nYU~jGL$ro}K$Zc9r0<}oi)s;K=bk6`V z!{wJDOoK!ZF&s zzP`dymoDqf)BJw**-)bM`c1S%Z#tXG4~MR66!=IMyPGLQ9=ZjxNDhyW=V9>{#LT?! zd9|Gg0mV3K-u}8Eu#aB^Bwp$jRFjk8Dm>+@<$Ch{Q~srq6iOWGx7m+wc=WmNb4vcx6KR<>m>XPFx0) zP@#Dr5an`9OTfVmkftJFp{y>5-TN6Ma{{Fn$wU`4J6VVboF2NQv*DS&_mPp8zeq+# zX6fJ%y)jjnzDoa1em~*?Oq-*qJq#r1e6;zAjjQn6Rbji!bBmvx(&VJui#gBQ$=RiF z)aZ*I3iyUkf)~JLI(!1VBd%P%iVYRL1H!A@n~htv+=6?eFeTJqh?0VY*bU3m>*%)~ zSdp`k6GXnnKp_tA@bECga~B}jg+vw+At1=MTiE(!NvN^u^{#sGwguoB1^%)u^>!JSENkLe%@#6s}Uk$n+Jf)SY@g6`8aFU223m6v;jRjWyCRu4#ZL^B2BLRV*FXSaeEcdR`QL%k4ke!_$>agaD! z1O+J|G!Z#dq5l->2lOON-Ph`2P*0@}XD@)}Hw(OGd90l2nV{1{O{t@c2swuqtro=S z!aUVZeoIqg=OR+yktSz(0{a{ce=ckSxA(jVcrNg|#Q|rlYZXpfn|#OB|4Pzi_K#fD zrRz0RhcFk8tkHl>4=yokW=WYz{|SeG4|qhB_B&Z}4&kIYOo7PUSV^am@|;Ee8p!hC zn-kE69ghv_6^npLxAN1x(xO-fG7CS-95}Td*;U6t*# z33Psh2qDz5v~EV=fvBE_ttR>h2o$ys4mO~1XYk9Jc>e?>4%HjNtTT^KeDpFUo5bQxqMr111i*=qzzwoN!$Y4n zkFl@A#8W_f@Tm=stbg{S53rS3EthVNr#mI8>0d4<=60Dh84Ki@zMh`eDUTLHO-)UN zxP!tSURcNhqOkV$v)_65^yCS73+bzGBq^f%o@`^jsEvo%ckiq1sECLwz?7h@e}vcO zxiuq+Xd<8tcf&gW3G31R*ogKLubfTsYp+lIp9`NCU^oj|{u;L*JXO_kKAl?9Pv1mTprQpQ{6oVFWBzjX#{>^@L`tPXw= zQMqyU4J)*sn94p1S4nhqMz^d<=s}h9-1;a>B~Z{pCtn z;PxyAEXUccEsIOY`#&0qKSFf`FOQV#rAunU8Fq-n5&>Ksj25$FG~#ochoW3%olg=C-PkeWZlv zp+Ga&7spN{uSW{tpAbx@>x>d;%S|o(clLd>d2!KCIVQ+w6ym7nK=_7i4I@&0q=JrH zKYu1b8Ux@~RiBdoSFROFuEI7Pw4WcPAr&duK+vM*dO5P>VJjFv)Lx`!!Rr-t1egh> z(rsH|QK?SPellFo5hXvfeS1pN2H<9YyJZj?9o?xqp$^vNxjr?6`Bo$Pm`_P;Dhg%m z`cHvTP&fRor2iT7E+2F>F9=2;lYh~FCIyTHas-1zr_1EQo^U~%)Vjt3sv$=NB?y#Z z0Iz_}iwIhVeO=u9Ua+?eks{SI1v)JQAc2|4Ag*6x<0wcA;lDhfKM*pCStX`6qYPrR(*z5=3%zm%8`X zmEJw?a~5A{Jh&xO+OL2`_w(j;mlnodSGTZf)A6EF%39ua*HB3taaPA2ZPy=wPD;YaTA$INmNS+4NiFh$t`+M{5=sk;vx; zwOr7E5BJ6_`h0_p$YvlbM$xzd$OKlJ^jUH(Nv^11g!lF=~=Cj zKYcc^#lKs$tJGUb(=HW9TSHpiK5pPe$&NqYuNEE)Ia`M|#nLj0pdDA~4bD*Hn0Mlt z^{MPuLs>TtVf(1vtIn{o|0wW@mp~#X2G>T$)Rf+TcNrEpV4YjO>yCr&5C}scWSMEh zL+Gs8P#J*pB?eR8{jR(S$=)a6VvSz#CC;8Cr95^J8DIm%E$%{&=1q--j)Dvrjao5Q%tq zKrJN#u$K+kE683IaPKRCyIBypy#32O6zt{Fm8j@3k(^_ z_#>nO;wjO^#Rxfyf=C_3Qp?xnwc+o?_u)cFnh|{3C}>3}*xi6sJ0zqG5FiIw-y!HL z+raoxx0B1|mf&$S1nlq;gBNO=^FauNFDs|$mGIKhJ-24MDK2gT zOrz3mN;rH2svJ4I9A2LvK$!59l$3Tlh@~mkOp(0ffW#m5N*lF1cPLTv&d$!22Y#j+ z8i|L1tK|Xdze&u2@&aKYNt)lzV>Hj z(d~av_6b;48p>FHeO^p`%lp;&PV-u$$1Zb@zKd#*x;Z&)F%GBSno#iDo(&TXIcq}0 zpTW-+^GH{5G8fMZhA6h)>%P&;g&xKtDryK`Cgr$0Y%l`6t#8_JPXY1+w}C4?f}*N< z{udZiq|O@(Xgc#iOGqIt+}_uB2WI^vq%gb?f1t5G-Jm-@F%b(7y>j3(m>mY6;z9*> z?>rbj){p`QS3Cy7W-C)DlBqfmYFS*UD$MXQxe*J;3j|IiFNy44#J8}pfTGF+c}W17 zNQTjMqDtX~bci|Zsh5THTO7s(Y>LBUpPdKGgEAn9r~{Z+^;sX+2Is@*hduOWBufcm zv?(AL?Jr=pz`G%(qjM4b25%l8l?zpD9zZe)`0Nw_rys)72?H*|0&fwvUJyV>hm9(1 zU?Iu-`@a|U`<)Mo)cJF$n;#*&@)0&hv4BINS8_h^(A)r|tdG4rb6TA&z~_p^~%L}{7dKjuO&Wx zU{0knaG&zTAkci}Shn=r&&F>7{gQ^O`RQX3`gR)VQDOmwVQ;g~r)sBHs>i)N;3Xhw zPf=0P7yzSjuuc0BfZNi+ptFOPb3EA1aJm2{00C+q*7vYS0FqyN^5h9*;Kw{*cL^C1 z6+iO=$@Mp(Fd@S&JpeNX=b|2D4G@(D&(r1cAh1dTSS|?MXO18NU3MPW%!qWk&Kr3U zZyu?mF=7xRTxVAADT5+`TJQwY*2t|u7WMM_0q9AOU_;O4guCh2f+4_gyfpAqoC6gD z(#Z0_+Ulya=NAygkn~P+TAIFPQ>Wef`64s`YDPxJ<8IJJNpCvz3yyCAQb9HVUEg1W zPwB`&FTH$lDbee2+sewuMtAk+*Nm#GO;rQKj}45LY7qi~_!LM*qrPF<3RD0*;uug1 zFAS#9HtlrL9q$7l_(vWAA@y?X*Wy1ILA&?+n7VPUmlAzBWSmjF@V zZiqUk_>N~mjRQ54OkBl>qhF&8UpF4e?;g|iKQmQE3lf*R?_Y zq2ft}^EMtdkcXfwSOY4AGI z4&aY0pq$+#NeKzqC|$}MFzBYHqQWnFz97%xrjmq z8@9trO1Mp%1201&9r3?JUze8h+ya@>R$5}F43-%Z50Oek#3@8xI)VLH1+9knjqBV? z&&>@Kx6w>LDM?5|0a+nN0rH_J&7gk>jAl!pozJhVypaxp&q#2eX)-DqyF0!n3bz2J zS>Czy8@M{?ec2F*1(jK+`-aGbhp}$)Xoiprm41m~7;t53kOE{umEi|!9`r%O3#J^! zgUx9qI|U1Su*yyi`9vT>T0vCy2UdZt7fr~%FmUX5p!Y9;xj5z#0!ipS=_Uxu{iC4& z{ApYN{ynlvK`z#6e#Xuz#$2-(k8Iq1{1bCh{$Oh8jChT<5d)R7P_^=o&D=WmvIXgS?fKMJEMmDF$U%rWKlSaHnR>gR}jMMp@V`r6=c=r)U zL4|IM+Z&CFd0~*hK*ll<;myUOyzf3P^);3X9#AZ9Tu^E0(P;~sU0;s}2@f&F#sRI# z%E^&3GA4qKo(t%%)9*6YD+}t5g^)n%rTdiow*+Yk5d$9f^WOMz1jz)8`HVXXpeg~a zMbcsLhYg>u97L%qJm=cU*qcy%kbHm}?OOKsJIBNd1yzA1sSZNz(4H&g`L*UAWi3k` zW027%_No|0hYiSmpvdxWM|lAOeIe?F z2|XDH^%HhMfT;^uE-1$f`37*@aa%beNWsU`92+9s1sXL?;M>6UaehIUZzX8nuecC_ zFG~391-rK5fcRvCk^r8d+&kw{*b0D1!Rg1QUGRk28ZU2&f-u+Ig4$T)?7dHL3s`x0=j+pXPyN<>0s zsFb-uN~T6rLK&MRLzyc=(jX;6A#*}VX+Vl7L`s>JsX>NNNg<(9h>+>rOTF*j`+WQR z&UeoBU8iebySKvsH$2aJ*1Fey--{3}>II-V&R_)3l~+A*U^%6$0KHm?#FlR9V=qnO zM*1Sa{oJ!=5kSvN^6WQ1L0KVzrnGqhLrp^?4izP}=K%|JS8WuG#UBT|i#dB>g%k@| z#-=W14zHx)mXzZMBDBOwd4ZgGLy~?A5Fr()So-|mZxnR%rtUBDIP!T(KisZo|2B&Q zZ78XJy8-ZEU>ZT>xJ=ucsmOeQ=R5(HL0uP|G4mm>IfFF}P%Ei&`-KptU#!>$HnoY| zZ?YT$snZPDReo)N={cOd6jWV+5J>N@U5tJtGvcrc`YpJ1%diL98-VB`U)9Js8LUxU zp1({jdgkuSIn$&(I%_sO`ztvG zR&#+{SwyeuK;vp;&E^Pzz#R7_nYp&VxGa65&&F+NFS23cBB4|jR!|T?^T4L@fi#j4 zKjbave<}FPfmj~^EmZ<28;Qq>Un=6vwSvY3V+Sf_B#!a3(75cu5dl$-$Y>HYC29k8d$c=laH-$~PFX3zh2P^pM zrF4pmNG!U$D916kpO?CjQ0(+Pe-tKRug2%8P_wjirs=$S3v>F)ifzj!h9x;%nn<}Xo$XaU7u4mulgTuE)_C|Ql)I+K6iXmg)jSN!*148z4c2o?u{O~R2A_W zMqDZ`yB^FD5f`~NqI2l}76i`j7wf(YEnhwZC-thP6JM64nC0gcZPv06&fFfB-7sd7QRX&lp>T|icF{oT{A4HceLjL}tx!IzkX6rN_gFN%zkR5A^v^6>o7-u{` z!RWv3VPR#bSmV?+uWt6_l`*A5CY%fgUs#Rwmn%#+Z>b4??vPaL3;J077K#z;Fz*4d zdIs~yhn2~c2=*LaESx1ss(SirHeYHvqR-R@rjb(%o=S;=&cSz5m!ISwGm}2NqRo-V zlVzn=&g<@FyY8o+GBKK&x8{TIuZDnG`Po9n$~!$mZc5(kQ&TfQH{w1b?*n*gAld1D z!e>Uce2n*_(~IxVs}9)kJZx%3W!~uavXLIw#F`rO__VyCXmN(#e)*N-_hwGMG<`$B z-8*C1SYzO`>>M{-#p0Yj)(GdwZCGuwv9T^aPx!W?KcCI1o3Lf8#*rW8>z2)_+Tg5f za;>D^B;|xp`m+nWH<^sgDH(gjDmr&Ocvq~3N!!|ysBhLUmpd}WtQumzz?fwei0Fju zjpM=!3ns8Ip3m3wkJvYIci+74IX`BEzhhI4=XP=K;g;ik7szYj`t(Fv8@ss6fYkNh%bcIZ%Zrn&m5f?dsZ!rafAh)Cc%9<94+scvr|S z0^g1Gc;C&W6BzNY1sI&W$rn{UPg{tJWgAZ^Y z5zT?worvPSEv@%ne4oS@qIzBV*POcNFRt-&dK?g&rsw?T{&!qX%PFl#yIDUD53v}I zsW8uzv1Wx)D%dB1)LsW`;|Z7~-S|CvU8dGea6R{~NmY$4fMmh3_lN#%V-hz+j6P`N zQQqjX%=^yCSQ4+8IrHpQ8*utH*A#}9YeBH@S^vG9!3cE{d}-5Hsf*4v{_aOtyte!0 zul5`o9!6s->eFMz4xX&gb+pZJ49`H_hiLeQ)GCg`;jTqv>Q76Ha6tO{}T_g8+= zknlkC1;RiLE^2DY$jA_o2eC;!S{nS|5Y!(3)BsgFCA{4L2B=~tD=|FNZ8pzV9cU~- zpNbYR*!&jo`I;JNob+lxMCI4;fkb?PhBqMR#?c-R)S@=k*QW^x2vBB?^mSgQ3Hdll3-3FE+QgeG;;#`*( zos@s5-jZpprlGmxa{v3cdzBXtjWLTABrNeTXWF&$;Xp_4n8dg7=mU?jWAZkj0ez~= z-rhdVrN=Tx(UljwR~-#_kO8eg;cl%E#@>BOW`E4=>AYhZ3Z$&Lo0=NoA5&l zEOZCBu1@JI7B(^W$=QuGDJ7xN(f-trz`LTfhn5DmU`zDAE9=3=VwjJO3{fhRj;fyu z7(4;NEXVhz)@C4RTmGpgh+hqOIcKEz>92P9Mw~?hmkYTP^?iGA6D{bk6(N~oSNw+j zaRv6&2!15JVLI-R)yLuL{LY>|6Bx)&W^NEt5OW9l8b1;rP?VucSBEhS3&C_U?pCDP zL65E#%V`j$pESflCq**_-TJs8((pw|R0M7~^01usMaJh53< zU+wR4icxxfX{AF^wCuiaz!7xUg;zbsjBeHI^Gez#UJtlv`KBti3Q;~`arvtY4wcGGYz*36)S9HS82erwrBM}D zH(>%Bd}qA+UZ0@Nhvo+}cQ8@VGP*b7Pc+S48@YTpn~&-#EN==< z6e3rkY9IiEP#Uy|yr2f@7`6ggj}UaQ0(RW0-(f7!d|QoeUKR87r+gA4Qeu&ETs&nZ zuX$8pug1Q;x|hXMv>@^Tk*%P-b^h9L>e@9Sa>fMOIW_(W+~V*o8K3KJv!hBQOo$G2 z8JPvThdV!hybc{r0eUS>O-)lp1As)qW>$K2K9ROdQOT^B`vt%vxkMGder=?@b~oyN zAhpd{fCxqE+qQ)jN;^0>ASw418E)=zMzB&xcGZ`@>%qZEXw67B5U+>1@q5D2m)YNPr|3y+VR$*E+qvJb)GjIzEh5*x#`Ya#8ywPQ;KifSUZ-Ks= zupYi_yQA-D zDzvc#f&tYtvk{;_9Q`~S5HTx*Xnx@O-V8~=egg!-+T_y8F{%V0a05W;fC1IuG!$zO z{5O{(j=25mTMyWp3;8{wv@rNJh-u4v)!z=m!AOVw>{T=1;_~kKMRBC zy};BY(#ExKq3(dd7E5*g{R8i;*X%XY&%3I&bZhht{-qJV9+U2#iCU~&-eyr`2fB<% z>tYM0XTRQ8v>`X9l(Pq-fOwo~=ns@2O(3c(JZTsVvYtciwgmC6<9)s|9Rld{l<^kh z%}di4q4tGBaS-304n}9ZgBc9Oo?TwY2hu)#J;czWAi^BM3>3QQk92O3cO3PMu$e8; z8G};VG}H;iO0q@6S$Aez(qrO)0J`a%^?{%UVp|e!3OvXPB{flmQ0tj}uK$Mm5&#B~ zI*F8v{wBdc5IbE1KZn8#4oMCyKr^%VGX<3)m_-0kN8jrp98VvYlzh^DDJm-3{OQwF z*dOU!TFHT<J_5B4{qGf&-Hx|2ulK^e*(K{E#W{SS@`~aeq;D<#yK_S{8n?E zq8HX3TTX~EP9-#l_}0oTK$L?m3VL2M&@?DSl1aP9Yb9EUq!%GJ9O;zMOn8hi8!&%6 zFl?M-p%*V2e<+n;&c4^hJNBzv%6j~V4IBUPiJvyJ-?8qvlDySCZn(>4oK5+Ms;2d~ z;YqgIdpEo;6x{g6c|E|B-d9XUvrxz)V=-}4W_)F5JQ&YE`XGNy`?{R1|K6+1?#vz) z@?Ly*q}hpCyr9{sdCMj?P)!xz_mi_+t*XNjy_VDG+Y}!?fJZbA@*RMVkcJkDwWrD9 z>|vcTW90?rA_3B*z!u0dc#25?Qr2m9+~(p9gvA5M^uJAEPf%zep^Ig|9_4Mcjor?XCi8>RaA zNYLOM)?jvZ>&i{D^5=@&mE&3aUujQ=-lQT^K|CXwDZ?RjhyoBiYTSzEfv4i>$)Jpe zvEt}weSl!1AW$JMkb7;%{l^ULjo7MwKhpmDp>6YL3SSQE5)Oa`b>Vhl&WyRd!jV77xkM>J^AYoth$H4o`qU&|j{!1q| z2`4~&y_RwyB7Yx`!EZG_g2uRNODTDSM_nZEK{efKw$Z$oe7baYE4 z?t5%`-o$%L{^nMfCtKShUHheAE@`1^78bEvGW1)&iR;AbdAZv5=2xc&1A^80#&P-U zn!-+6aazl}xO`{wydU3Qck?z}aOlap0Mm#G<=^TWeqTZ>)s#nglV$?stW zpX9*6!~xydXmLInR4e0m0&k#|&HX? z{e`i4Zs})@I$ytz-5SE!weB(r%(a}X&L*LH1MApuVxwNxIksx#=;|3tN=;h~%TK;K z?#ns0=&n=u)$!)R{#ID6bO*mxgke6R4&q98zHQI$HFt>LtJao~qHKbTyls@Ra5kHI zUT8h!*(giaBCB5crU~abh~hq3OFAwv%pDy$U57_B6SpNv{*c%vKRTxN!R=HT^k_W&8YYom$Mv1)F%{HSniYSQYuIVRW$y6J!YrJGNKt3}S|#e?;Xz+h+XG1=Czz`%3zJ>cwrz5; zteW-4Xs#Ilv)XIJyT1E(EJ~4pMYPPq@AlKTuiadTWW$5INSS^ww&Q#M zA!n}2d%Fn^9~$krEdpvm8QigrjW6yvYuTAL;ag{PSlPO%@^R^A_aM%F~OG#0L@u-y&erU_2if096S+F0Nck;`S(_L=%-fjb8zoiIkNq4b2{|I zC*13vKZZgYz`aKMz`ccES(g3fHOM=B%I{;rr8+XRD~p44j{bvO?f&}4zx?w4vc0QU z1Lb)tNY@}P6adSH%4&HgyiOEGMn-VddE%I|d=kLZHan>^;<^-S+rP(82#7|$$Etk^ zdu!zEED$;YKUsQ6P&?<3#}+z2#B0Qpk!^hUP8uU7KsZS+8 zpR&@iDG2I2%K4^En?`*VG9V>!M^~z8E|C^ukvxFDS%79>5YiwEAS-PG*&|wS_|}4? zqJ}zeEE#{_Hap!G4tAvUfTx9R`Wuta@>{FjU#4e%A^#2-dMiP~f9k>oek9T&uyw+W zDCTcg{`G3MzsM7Bnuv__`nz8m(&LFoT$qJIIDia69h5;(smiq7?&Uxknga-IrQG7CV( zEVt(ZPgnw6`u23zZ-DmzgCsbAeZn=X&WVqhmip|#1YeN@3AI-j70-HlD<5vAaT7mL z9U+U(Dz}usSKa3WwimIyNE8Kn5M=1ofOA4#*~^+;lvZmOcYVUUPYBx|si``)3%mWk z&U!Vc;ljCiYGGGzXHQPkN2RI2tNQV+fxfHN+okbgeLhgnK(gN-GCDfygk3=h2Qsu( zwlO^Pke=+}s4u4C0=eJ^NS@TQ?tQe2doXIwLDwIGN0j_0xN|?F;2tmgFTwqfRbL57 z%aAnRY;4|P0}H^i!mvBOaBn4=2cb|XbavX9Xy+}|s^%;)tfoa*=)H3|NeUO!-p(}cajoeLleP2MP4yWZde zQA{Exn&G|u5nCi8C@OtAG41XWTQhW9w?NFZd)LDwngAsYfJR|eVN=N0YoAjG3k#cr z3wwvj6XnV&`U7Br`~T!IM}R+6WIPCeB1@KNjz>gCCplVi(84d=mWp)`v#N!?0HOjx zHlcn#b?dxrotsYn&_#*C>Gc!eN#u^A7ccYxK?j1PA9eWgjqPE17ZhEy&;<`H)C9W7_5x`H8Jw|RCJaE>G(~5(NhO+sFI`|tx0#XR`Qib)CUQd`B-@cCF6(?I1^L!HmK1`#Q6dgHt- zBqN3}ogiHc3UQ<9eE##7Y*UUN-fVk;aXKy*x>o~r`0@3oS)LU&i-3TRpk96v$c z3I^6b*BoS$JlWNi2tdkr=w{E#4ONuas^^%;nLV?t@ZhFV*; zK|oq)mzsNqrb&=)2|JZW^&p7F0S?r#J2C*_5?(+&uwv!~#{k^}$#VIebd%J}NP|dw z3Rc)T@dJ3(EtVWuf6{0$;BMs{n*^}5z+#TGFU3xgCyb4TXF#t-g54ef379$f)CMbi zJn!Wp(4hgA=Z*AglkGT;o)X~AX&}M?Whm!e1sis!O{%OG--P@UdvOgQE0uy`Qf9=5 zQ5M@iuu5@9lp#M};XdM`F!V9}(+t>DCmO=+4UZB|nWF#W^BezDf6M=0=953Yc!!(0 zfBNtauZQLVB5N-ssAJ^o%n4h|ml4vSSe;+eeDu(v(}973uf}Dpl+y%zEq|Lpe`Sfh z#%%-(Tx$bWR_o+b&ti5h_URv!*_L!j*gHF32wA0$j!U^cra%lOhYyNZ@rJvfl|Qz6 zZ+mNsO1}FuvWx#%@^(kL{er~WLGhj|*~N(~?%dGE%m~mbkX!~9bDQ4c($o1iDN16- zry0ePGKXtBx;x#;SyRnA9nSeaA8c-VhR>JUh_4R2nlgvpHwhC#v4O)Bzrp~T8c&Z) zP@)=V8M>?)GfN1!1YZ(kl{V{*tJ0h0IgRNm06x04#^F!Kaku;OUqOekjN5F|$>hD{X{g#O4{FI8pX z$f3bvd0oV^LLAkdFEMd-vf+};MgY9gkf$4~k0^l{qk;Jp$S)!1nsVez9S32dXb{E@ z@BU}|X6=bp4A#lK=2tdygJ4cbLl@kb?;(h5`x*og(i}{rv~+3B1Hu2KF#6w|^LKo3 z|1)Vv(^4XUiwOh$$_yss|F^JxksK;}w)N7wDY(JAOn6|p=wieQn(7yeHV0gNL5^dE zouYvO@7@jG$`VQOs58PLi*l{4~SM|(9{p_jlP3)9AI!u*^PmMClYB=kfeAUFPiV&iV zsl!G=r2j4M*9#%h5^~msnl8TuIscmAhWyTEr=i>?Wg%MEB+JIBO7yw-VN@l;xZC$X z@yH9y%ad?P$&;dY9=%h0GKTr=<0nPN-yNR0DpFc!YEgGkA_&>xVc#vnsOB&c3`eSr zhAIp$?TzY@W9{p^kxK0{9FpN*)HiMNgL}X6TyzuA zC1mB}i<()w)VrW&^FC2@8a6AB%AMExqU*ln)-D16Ky`C-mAs^{De8Om?zmqcNAKy3 z9S@7_Mr%_%PMjmGtZ7YlOyqyF z39G1!irC#W_dr5*-TvWbr@E86A2IAsLR&d)+;3Bs_b~{T$;vMs_J0uifs6{t!E`WS zOQ|Ya8$5{^1Q==unKa1l)s7)>1Oo>%z%2$VA0E;dMbsvCmn?P{dbTuchfL>*_X*k= zy4d8)Lsj#xo8^m8e}G8?q+~ucfFan#A~^rS`JH-g*+yhVQ_<^XQCS!01syfzB_Igc zH>6D>%PUHoF*+pWNj%ah>GJ2cK4e%#nO#}gCM1L8!A0Hte;5{pulj9R^cWTa zWDeiqmggKMw*K}fk;(Mq^~W%jmpZ^WTB|)aPyAICqWr5@z%?POqXmcl2dd*+yZ+n&H*OxN!f2lr6t56ourSE> z1Q7Ol48^2h;v$#?3P=g`Q5H|<@@8BD;OJ78(c{(A6gk={^-@KG~_w7|#^83o!> zxx@|DJX?%R4oBNXNpTL?$5ErDcKAn*ZWZcB74S85fqPJAt9XL^uYSe7rX+#H^?*>KuV87O$ECs9k9-!fY}ewZ_#tPTJYL4 zR1KXAYV-##d%nTX z_hlvc&)6rCyovG;A~mg$mx$b5iE{}B>;i^48k9p?6B$WQ^| z`t#d6DoWwuBVjg-#^^QF2jI9&Wq_nU9i;0bDCu-AcC+0pFcTb$kb3<}X_^Yt7~2@T zJWH+j^>aGBEZkf72v5x9yZh=xU!%&_Ci?z>^j96WRg7gMo8Yjtc#mh$+Ztp1$3 zflmUnW?6mXdbZGnqr-i+m6er6>I%EnJZI$1W*r9(Th^Ze($W$W9Za(-kL}$eMxXhr)gQ9AxPfJz19#t~v9t+poPKVwP^kAv?8{KhMcQj^aRJmcA=Z|~u zAt*swYhJxdT~oN(UwyN}$?n1MHy&5~+>0bId>p^Fhb?Q)#fb)!6#tWN<>v-#I`WgM z^gqg}63t$?fHXn#zxuj10;fkVG7t128jG7Z_R9|76B`1VoKetP3}jLbT(->Ito~qT z1Mc{y82CgYGTg7V=F)MAAVH=*Q{TePz$DYIUhhuLM_E7v6b_+AB*#4L@^oMaZUUGv z0tH0ved1J6^8sZTITaCbfSH^2y=z>K?&rAUb8+(imKTLc07^k|!b@H@v7;q8v9C;@ZXN$E96BzDSp@uP=^vvS|5_Lp#5U-?; zobt^S0LXJcjnT<>gg0K0)h(-z=G|6C?!4U0i=j))XRo?|cr zC}z2lv17S+dfWZmL0n9)_Y27`syFxFZ5I|D zzw#KXnzb;td24HH6<91(86(*#LQ(<2^;yD6$xIB0{l~RDp-(=`t!ru&)@N0E!PfU> zU#?LqT#h}C&de8kV_9994%Z96#OiPyM8L2=p!!ya?gM^reQ#a+Kl|X^dWyp;ur*P? zAk|&|u{yv z#Q?B_?JwqM>us=h0wuI0baTriL3gz+TZGPQC1h`qxddzCwI?6G$4^-R#5(eS1TbUpV^spb3IJNY2N=Ga(O2-*uDD zA`uDc*X+fF2B9xf;bu6oY_^5oz7B-Xv|!&=w@AlfY?mIe8!P@!1yIVuo&z+COOC^` zl?q50Nw%_LjT zYQn~on0;_$XE2(Mx6h}X2-%I~k$gh`z&5EIawjne<3VZAih=~Kg=GiLU`+*J)#T@eiRa!DderzX z9$j5Wu-%P==;-6VK7(R{Hp?s!Lng*|&uT(i^Wf2=cu<$|l37rSPCIsNpqKR${fLTa zfC5+uu~jzHXKrDTi4jmZ&{bKo^_Usd^|*Nb;iE@bC)ov~)LSBL7*MPN>~#`ADvb8k z5OIva)^C3u;8yQ+U49p>`aNI%j0DFds+T}aIP&h-jhQPeh?G{IkslD`w#B)a2Ump) za<<#@CInRg#aFsx#Ixo6MNk;wZTMuZIo>PAPzk$!vT~Ym(1ChX9*JYa*vWBN4n)`_ z@-xbviCNzjHabYTyu!n`$ENd&FI;#-XV`@8r&r{?fel%6U*;}*@~1d`c4qXPnUVX- zzb2}QBEto$7mw0*DC@pm{S0_hOZ{{r-y&iQ`(*WiE>7%QKyaU2zo95X_OSTp5Dikg zwtg}V4PwrU{tvBw6(CEqhj$&vfH5?ug@l9{I5_o7)22}4(aQj1j2Dgyurpk6!_CG| z3ZHRDvjC}DlJ}#NI08KxIU>>7*lmi}T5s)M=zVx~u`Wg}pAHqW2NCm2{7b^kNE-K6E!8uAI}D zRtG4RY|}iQor17{TcBDX&O4z8)aS&sI#jV?iW;=>Wt3lQxZnwg-~;I3w82O!#s~1W ze0+UbKq!_%3P4m#VpTIuo;=CsriRJGBT4HofmioA{2-XN6i%^u$UgN04CYbiS&%Lh zz=>uMMK5bpL@#gwpU^wQEangJ()I%QOOhb6;#)YpItiUW1c42)4&Ge9zR#aOpY-A3 zkAH(@;VD154bWDgrX2!cXn6Q!#vn@TNj4kJexH4puyatS(D*w-kjbE@&m4nC?)<63 zF4?cT?Kar#OAQhv7VMx7#DEo#h9-G%;jw)Dy9!%bwz(7E)%ngPp zdb7w#*8k&&isnEN1i+d3+GXtlkenGLa{hn-@sIE~uh{`p$H@*2;jRTPz(trYK&EZD z+)pM!rzc`Z4r)Jl#BeCJ8H|%ye=gs$+sO+EATm8l`f(|b|2}wmqu1n=NBM2i!()XR z*{|=YNc^y2T5)R|K;P!rYbPWkaaJ#*_4$01twyYl6tw}46@nh7^ahxqS7iba6($${apcLdAu`oPg{)?wAai8+w0!so7BISXj0 zpyW)%NwqpZ(VPVGF#(uRen=AT0Lf!}ZWOz}wETOZ<2^x)(X@UycoWlm@yz5Pj-i{W z!T>FQ085}Hf`H@rZOgNOf12cHsmv8B;8HX9_qUw=&pUn#@4~d?Hydvfir3ZPam02Z zJ!)=mXTdKLSE3eKd~=g-s+b!$A0LZmSg+eC)_5C#`pAs=YrRQp0*nH!K+K2U9Xf2- zLY#oK+AqTJodZ-U2EjiiS3sl5i$nd}KHGn7i>Cdd^Z|K9XH^B1-j_(tfLY9S@c6^$ zL(;MVZly}_;iHkx0GyV!m$mJz&U={+<}v7zeP#fVmf!T#1;uqTkt_jJ1iosbL2^I6 z&L-^U#D=0yj#JCyX%u3C37k503afhY!3jP^4d>cOXI-vP|7l^n(bs;0Gms-o9>YS& zl*wioc!xh=$hr1pE1`~RSBKZ?IQqtrFl=FPfugv~x8vDl&xSlV*H{P1p^Gn<=qVc( z_X~M*Em^XJ$rJ8jQfV~&j93fV7++^9&>Ug(r|iWbKCdoZ453R^ZkXb5CDjd$Njy^K z2(3ML4RvqJxy9CC+ylV8ID8a(?$?$Sr<{}QzqojrcK!l{;0Y(##eaRmeC>St2pyL_ z&geYB6UW8Hh2MUlyIX~r32u;924_`*0S;}n?r-)3j;HiPZCa!w1USBlC6mlz_LKbqDd(M$n@1Ob8Pz|B%il@;)mLgH286 zfX(u6P+E*(c%<2yVuOdYcsUpxbac{G6B+<<_coCh{+jSdj|lBC82R)Do8P#8{o}mL zRY|a9t@;VD2J$M|KjeWMKz+&Lv28}GYDTS{>Ws)EibSraZYs`u@QQL(-x~MIZw+%_ zQ4&TR6b=k3%=mjU>GZ79$B(J6p)t@iFi=_Vjyj%VFS6`E>jjoBlg8#JNS!IRQu z6@Gkv$@a(OH`JB7E4z2Rt5J88zJp+C&-)dXL2EdIcq4TZ!{g567O;w6->SML^FqON zzEgE^>grNqYENyr)z0(#1^P+vl4G|zcw%116Q|C&sj{c1ScMgZc^AkemV9Ya2vKMX zKK4ej>2PJ|@TrXMn5T!w+AHr3IypU;ULN%{Gb`&NxIGt0jpkcyal+MAwx7*5qro)u z{{2e_SKDi7XgovYeh$r3u-IW7qB^;%#yYyX(Jl%HT6!LYTEpMT68~|0^w&M9Y5LDVKoo@InC?uqEK@Cz1D|g>DJxkrvKF2W9GFfx9uV-Y`05;&iSz~)E zIl2Ah-kS*tPmmM1y%;o<+&ki)@3i)EN&AmzV4)rlS`soYn4+s&bv_~@8t`%S$jHg< zJ9qAb^XsKkw`#t4T;2BTM^$yTWzdk@r^lAV=C6j%-C_yTS-~GO|Hry#{$`!Hswtic zdt^W5=bvEUm!zL!4t`6MW0Imj1sAa6H*L{wCI+LRn)Sc&Pv@RkreCmMc}dBAZq?^fz=rD-twK0bgon8$NK2TV&d2c{iCYmei@l`>JFx|X5wD|@*qx(yY-c2@k zcG;gJ;7ZsYI<&37zkd$goIS5v`0$l6fqqn5P~f?C?ON-M*ZAN0O=9j1;Jv-Oos*;R zF8lDth=_QBb89uI!M%}{UKt)!%_ zlDxyVyWQN}z;cR0duf}g^099*0fB)L;o+jG^~r!QuE)>E!(dyQqr%(K(ebjZ%x|TA z!!;}b?JzbrHo6A>b&86&QLaeLX0zSDUyFl-W512f7E{wJ$j)v;dTEO{i1)dD`}T0L zPSgfkVlyFj>OGOm=3D$G*Y!ROlXsY!Dmm^B-J}MmYMtY^@893q)zyVBGR6d3$Q^D% z6rBHlI)lOMU%+lXDWp{NX3dhQr~Ng{rsHShtw#Zz6Vn8x?{jBo6{HYt&X0EN*pcw- z*NOgtf#jql&G`8EhsS^1fR+)rwN!Jzo!xf1xjRO36dG%gr6E;-ZR1_ZGm zrcx3T)lf9`!+;tD%Pl~kUV$Ylw_-*221WchaVDvA;xT&dF@hHp0^fh%DHC1$_^~&{ zqgmI+hx$6hVeI(o_3NRiRv63huuhrHIt3+wGypWc;^N|4ckbwI?8HxDJ;hfxQ|anv zpq_YVhaSEAaX{QKB_M!pK9s`GpFe+ia_BC0rP9g4YnT;l2he}XK!-;B)`+O6l)tgMxK!z3_R7vZG)gQ~(3p#w0gj~hKH35}q;Tg9lvokzX3fQDP)uUr#^zH1D!F-INEo#VR q;4+E9Ft<(l?{D+}hv1N@%-E``+pWy;jtAe(&{Wq`yQ6A;`hNkulZ6xj literal 0 HcmV?d00001 diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png new file mode 100644 index 0000000000000000000000000000000000000000..70152eec4e097d26ed93c3986d4e97d217bc7967 GIT binary patch literal 39812 zcmce;1yomC*Dw4h1WD;`5fPD;5Rej3NFw_^E~hKzW09Pj&IyCKF08zBWK6jYp*ru{KeeiDtB)a;L_ltP$&ZVJ2LlCDD-+1 z3T^H@7X0QmLnHhGb;DU!%UR9N)YQm>ky3Ymxi;hB ztu%Ioz4cL^&<15qC%a?tV`0; z)?eT)j)tS)~els!Ibiaw|o z`+Mu%^0ue1uE=&8`1W|`htPcXRJyve&BxC_zq(rF6U)Xj^aRb0+G2=mdS>P-`PTcI zn&z3AnOxr%@^;=iDij??vujs#&T~ALM3(u*AHfAPxt3i9v-2J)J3Dmk`w?$Gd2y=s zhvl;(^V27J9!*r1L1y4qzJ`L6Tb`Emc`@~T0@wO*dNye>b)${hS{2i)gKT*Mrt~Qx z<4?8V0v&yGsaT?v#PL|jOp}sIGwlS~JBzzvxe0RepZ-QI-uadxOF_cZxv6oyvSu^K z;392f-5I)ge0!xlBy7FcAUQ1R2^u$Xe7MYI**3lMu3fIgS7L6t_q#Z(h^*>gjo2^l zJ7U$z7^@>ARBG2NzioVvXCzF(*oZs|Q;aeCq$nfT2J1wrq;Rp6W)f~} zkWtj0_1e9j-31F(EOJWxHKxBSYEJ9*%Z+e>ql9QjTR%oOYX;J=i1y+EKCd9GI)@Jq zVImaK`IoakF;`&ReV-u%gG2EwUt-BNXjYVAGx>3>;l$20BY8(M;Y(5e(y@T&(OESw zwLAX9Ee~{*@v~L2Z#aeo^zSs)bsg#89T87mrBJI=?kIUw$8*zMWNm$odY2@AkO!7^ zB1!Fyqyha(){vm7o~1s`4yGzGb?KZpov&2b-%SZfl_=!*jR>%m8^T0&TZ>6dp=*;r zo z-7+UfK{p|Vr;Kxjr$-W7$6dGMo@;vkkaFO!+#wveJ>5br^1Fx^+f;Jtm1*;B8RI{T z2bY~>u7vAm^9beij~4~eiZQoz6<7_u`2CW?x}PFf?*EKgOHN?l`#iMySc8RS2bmzj z0x>CDS&Zq{lqLlkV+Lg8rcVXB1=Rf&Wn`WOz`D?`jKHkLw+|01#)Ab`t{{Eh_ zA_*}u@>d3v)P9pVMHyo+xPxlagGpzW&*_^uC)Kj-BAcs+UnW;V}sa z3*!hxu3GnPAL_!>&iA-~3wkDofrC5j*7cnt4CUTSW=ggclbkYMO ztIM6!&3e#@i8aXJ_k7{Yug%95_fuv~?#F*OO#E7QqT)1NyIEM#@L2i58Wm+ba{WRw zxV16Ke!@*;2gc>K-$9{Ex~Ah+keC^*>)CY&+m@|vF2v9;1R9uiF+tGcfZY%uj>Bw_ z*B#XDalTT)Vq+tv4;JSfoSd~-OBKUKHE7Oys_RZA>D7H|)kgh@g@q3b?!v|zEV+a| zzwEt%zhSLq3a33qZDp~xsa!dK>qS&Mc^8eyM<+SBMN|mf0w;W$j{RdAubYt4U4q4r z%OYN@)(;fM?72Ky&78x-y7QH2woFY-9?#Z$+)+}J@$?iKwyYf3q2$u#F>wp)>)ENe z7FS!FT1P9FsP7_0z>JCId(6)@Ey{k_c8&Hj`7PopQLO)E_YlXc zPNe=cPESwISIw4NA?P*u){|sDUdH_L7vxEfWSk#_F*2(dbR8q57VSE@$ zB5kyklux?5?^utQp>c6>k&6%HD!<@)+=^MOTT23C?&@Oni}F1=WEK!0kG;9C_4X)D z)QxasV`H+b(sHEGo-vGF-=P;pS@NFfv*`PABYRWib$xh^wwWaxX@7d`;qK+t_VMEd zzEq{}zx928=O}fB$HsQi+SLcZv!S7$acT#`ZHz_=HBv;~Y~LP@lsuwkU|=BMkRbkc z)q>i93rz_&RwrC=t|jQIPgH%q*h^stbhW&@BNGfXG!bo2E+p63Jt3rzOH9P@^YiOV z;l*h1I^e3Zv9;xLbaJYU<>%iqD<#7{M}{jKa#%7^nsxLL;~^u4>YU3}GKD)D_H^(j zTZ&@7uI(L(Or6y0mYuAZoSl1*u3)kqX*quAf~2;#sDNv+jKoQ4@))!E`S}Wr3!`ry zsuZYi?T^0szOud^y*gI9I@8tFg>5xjEF~$4f;(?_Q0l+Bmmz*fK|%Us`pvx!ezy(X z_V#wNiAJ-J=?XbUC`oB)>74y)tSS*K*9bA^)6KJ^{Y%o<5?@qGm|Vh)JJ2U@3CUY? z;J@v*RFxjt{3|#0&fRIXl7-11E-FM_uN;ly($Z|2(~06M6M~6ahcDlB_=-7@eiKhV zVQzCXmXKcLsmB+qv64)^22Ww{Up(j-7?o3S>uB!AUs-)ZSp;;#L!4E0H5{Uz&G0?M@jBR$?R|Ewa_acxWP9*7BlGp^@A|r+h-)r;-1ezJ z?J_17mSO*Mfg11Me4SC$7^oCM8=4ZmhKP?6XLORjs`1?8V7vP=Ox)-2>8h4dS4_wFTcbVkYTpyj zw64U<_c|9pBwighW3keljqWJj5;M49NjjVg#<+HmX?O_HrA|gEFQ12EQ4E{ChFoqUV&a>ILmUKe z-@ff07~m>DnQwa%79A~M@*u4E?)(otUFPVpF!l@x2`MR5V(we7y7$-CtYL?zlwXz6 zlzl@%RmOSlG`=p96TA2d?-GkyDvY>XneELGQ}CxFyo2g5zPF-j1e7MoS&HH$ii$YO zrD>XX&%sj>Bx2S>_>W_S5TB`CVJae3s*uwPai7Puk4nusq?^MWedhZE#=+Vv(Ib?A z8zT7Ro9m1}kep}UI$SNi$i{{>QuJU<{Bm;d{P*uqnwurzq|T%XPfXG5W)P6s!D z$cc@*Zaq!}8z4xDjlaVpyja1(r1RKk`mKi&lkVcnL3mX|UCU^8u$7=%M?=wzI5)#- z+gtOu8f*mC!|o(E@)_Yn@(t_hTt+L+%exGVQl-w-#@*e0`J|(`HdQ(2K6Ra_jE0;= z(QDpMa6JxMnB=4V_0O6H<4t}|Z|~hUCa+PTfh9Umb#sn@kZ=@ce<-%-H8E5(&*WFP zPIo`w%h!BE6BrnXLNWU65{v&{B>i=Cw2&Pu9+;Pxcct!@vFh%~un97|Sv~wK3Ras< zXX1SPI}%wZCkJ2PIsvCAM+>bXcrZ~s@bWk#OmnW6B*?+aaA!-KZ*bpcfu)CrdRS%M z(j0)nYc@cKEX?IEO-)zoq>b+n$`6($ukkkYvaVMhQ$M=2ME^2&{fQ0}|CIa@-DhO_ z@w;G+>T=z7brnPzY|b{guUC$tPy&|2p9YRev@d#nV2GEGx9?UnWJL-1w^-f#x)#le zTCZH!3V`~qdkYE|taIm>Sy<4KjC*!^AYncE4hOaMJ;N^xa&PV$ZfZCYI;04Q!YV~i zmo{QPC~flL5PB$ZDx^owF9&8S&$cJg)?b13*i8mkL-r=JoneJVY7}WA2>|5}n~eCL z`0v;7Or}i#v+$a}GpAa%nxbF$apn^C?Ogn?$`-Kq>58ie$w8hYiMp*0DyT83s zCuPdw=mdbLb9#mc4{a_p4Zn>xdKjxtVTHywjW+_>a58eHL;jQ~bq?|>yDu>nW*?;! zSl?MskOW~71U57@6uaFDrzKwt7pVHg-Rb#* z%$~n}0v8+o2+9A8-&r*B7JrNsna|eW2qyZq^HBm2Wh4^n92Por*9cSN|6QD!Uk}*W zVPw2guA@Ux&Dm%DE&5C0Z5F;OBq*v~D8#AbVb5-D*UNKAsyUHWgITQ(y zRN~&kPyjF|en6pcL(t(>v<~Gc!g*P^SGjzo__KSTBX90m2dv~LPp})0HbjqM_uNrd zK8J$cmMx#imb-?f8j|$1F^-D3@6W*$-HR^nV7w_ie){S|P%KfB=;L#oI9kU)#fLoL5Lp`=6Wi zY!D?ytl+`X8c1M;#XFyPPrOBuyW#cBK170So2e=)AzL~OJwQ1@)88LZJ`xW6dwirk zhF+3F$lWv%@(BhaB2>=v_X?e z0A%rdJQmG$erLXF=`nY1%L`>YAULPnGgT}np!=ypQ4Pxe&o3r{A1Zn+ug%WYeU4!! zYKi4LlBwbajG7fSvx_Tgi)(;SGJZ(4b^qU3@(-B&gC+S8_}E>=JD0-|bOo7Hbz*$? z4_~*ju0xI~#K)>Pju(Cb=|Cp5)jv6kOXLa-puSwURQ1K$nnmIKU|iJ}Rrh=qG!*iT zEPU*6g7bgj!tS#w9A=_aVcH79fzBufeqIA@g|{oXC^ZU*i1}Zswh{*W@zZgb{cGB5 z-Bo|!GNWNUTkl-QUf-swn%lud%~`{f5L07g=<(qy6WX4h1!OnvN!!TY-rYYdZ3)Uc zBEz>w!>Wbh8B^iA7YyNIRsW8S{Md3W3=KIk6&*v5BRK-A8a1bJ4ey1Y9)UdS%_M!t z>UwU=$lE8pb;yH3ijR1ZI!Ey-ttHLPUR+!$A~^CrEov@^Ve_%6BP;6_i7uG-Foa2X z9;Sk0%xWg-DjAADhVQ<&A82fMgYgS3U(Lv;*X-?=x|q2xkvMtEp*L+{pp*!`8Cz9j zx?kTx;d?^*>vk`+Zmx6urogFNLxdD+BZcG$v|+dzapo>|;|4VhSlDR^AIh%C4~dtJ zVB4~-toFYV+CKQz6n|vLr8^h+c{^`j>kF~+GeN00%#}(**ssi0j?C-k10VXRONWf> zy-gW1AsI?u|7<&kS<9jN?jAg!Oz8bf7_-VR1Ru8s<7m}7UV87g83U1u3fQ|_mX;h0 zUTbez*17fS!yv>M?Jf@@rKiL4C*~-6Q3_b0trZ`|t%e(Cr#(r{(nT@O&#c!zaB^6x z-U@`U`P$U$}htW%F=>noHG_(9o}2^KG67HyjtwOW(TX zQWX*s@_2o+QiYu??t`)H{)hg4SzFtZeFsz1)4qJ^On+(B>-Hm;FW$hYJzHXg0X4r^ zNb=OV67UgiN@>b_nJUs83&J|I{d;PotKBpX z_`+EJk2{;6b;XADl=PgdueO~DRLBXeuC5+%wX?I+ZSsAzDLz)JKQ^UTp#Bvw>Of)N5X z^3x}kn!38=f>`PjYGHam8WdA+{Nw)nC+j-awrB^<<@6P-1AL-)hMbbbV|cHSDbR`9 zqnI-@JxwAq`tSVoq+x+mqL%GNz)x5j(4eRuDbdVc! z+!+4@&h;F3u8mCL#q80?dkIh`lm&_9ecEPie%pA_`{<^r>ZP72`TOiBd5$q*ovSfu zT`1s$lV}03i$dur>g4vSe$nv;Iy!oUH|DdrIDgxj_bW4(d3eh19NqNVe+9IFoKMBm zwM*BvCl{NrLo)i#@;)T636dgNlsNTP(u?_X{t&s=JycX?M3y9gM%yrLpLHPxbmpC5Ts&AcdleyryQ_xioA z6UU2d@XbLz&2IapF?xJl>vc{}rpxN+c%>C3&2@7W>W-pf+qd52%5_>n>o7<`k0D9^ zTF#DDskVK@`LL>3_tJ-rFHILvTYGx~eUUY|1}TpTWo3+4;g_*cB)zDkl`+m_ZIOuV zm3gDN07JKTa=JQ}g?G-v)*A^_sdnBKg8z)_&!UBb;^<&A8z!`@qR3I(Q^uG@!!dM> zZf$K%rODSPh^`lCdQ-m^3wv1PsK4)p5d+E-Rf}KWd0=*lwFQ9FgCFA&h^BGb*%!02 zvUW!gD#`zS8&sEnn9>uWH^|ZP8VmT`|MN8DEyZg-Jl7EhJNyFJHdM#Duzg_pbSHKCx!0o=sLd%>$24=KtWtl*WzPZrIPEun6hG-n*P!cwK~jlS*q!0EL1GCHs9KZGLGf6z0bWE-efUO~H}&=Sr1$-m*@sfG|!B z3_XmAcNS37r1(RAUjRhBJeZArxVQQQ7R{=S-}YD8-OVqIH$8WoA+T?^U^6m_iO~U+ z;C1*&P*lJ0%3)*r2E1I0rssk-mRSt6nwpvvaJwhuS2dp&GzjQnf1}4Bu&TxKKW^Sj zbDM1$%)T8T7YEf#IP6tp6BF{f*vdzEGOPHz{%-GQS-~0J&dHd)(!!0 zuPQ^#quhKjD`cRmde&3Xd3|!k+~<18&zLrd`{z&&2OEz_cL&~~!k~Og8t>F8#fUDC z3J}o8pjla2LH7p=ovSLkQXkKR8slLN+(k^d%?brfg)lHmf5PW~2Gl$8`;Q+#7WO?B zCc_OGJhW6Ir^b#3wy7EDFQ3ER7Pw{fgmR=|^q7>a_KrYYd^{QoVKU#^6rbp@_fnZ_ z?Nmz2EVcNLpl30{@$OvF)lgze}2ue`T*9d)OFnp~-mA$=ZJ4HG;2c{q`)Dop^MhaSVK z37*HBu5Sj-ibXzQz=&BU-X^DVth%A_%17brcC&Q-FNDU#3GwmUldeA4nt#HW1r(Xw zdR|W1=<2KD;u9M8gBdr6AH!E(zkdC6a#W`@q&y}7rb@C1FyVc7M$+6yiQ=QU1fh@|Ic+`e=`mU(OUMI4ZR8M6F!^1G>))kaFs zv6K^;);${N@jlrb10^pU6u^k#VRc?0f9HSv;NW`no;8@r@W*h0;dt5O$$j9_iGU$C zggC?^AfQqFs7|xnm~VT%`pfgxOH?u*U5~fFb{I}o*?a}Ui!AS61_cCTa#Z`XYY+SO zNI1U1s`v`K2l<2U9~K)+dHG&C+1c?u(uy%oe3kMSHe622*i0fLuny=Zft$7wASN=nKQcE9Oz11(Fmgk}|6>6A|L*{7$c=?6MI$w? z>on8T(@gRSwRN{-WKh9G2q%Z|eWIcR^F;B!pyw__074Rnn^0KTo!F}nN?rlYu(W|i ztPM53$>P!y7SoyX$jnTGbQcs%;}vEXAP4|-#Gh#32r_~^_9p}I^idg`nzoLOJvd@e zmU(>vW0r5|H7}GG4>;yr92^4b>gtrw0xLRr!|q0eHWa&1FW->ijGmIHc)eO3i--)2|5&D3mm6!b3rZZ9s1LbV8-bxO+hDhc^y$-?SB! zG^@?jdVE)yG1tgEh;CfS)V$I^L!X#BYeE82YnWmbmgCQ@&jnm5_Rfs{`+wETEss{) zG@pOyEmj=+XA@3XkpNW!0jhjB&m8w#N?0UAG)R>8S!$gqn-EE}y>r{Aqkj95x4Ux! z0@q02Wc>^>06`U$GE7lXMB$P7*5RM5B)7OE>9G-2Tlbg(z4?|Xb*Kw~p#-J^9$F+|~zRg`H$qApw)p<22R`25@-nhOhjo&PbO? zcd>63A?NTkEtvSwnK8>9lO`p}QDj3J^$gy0v|PnO^!^_LHZN8^D89Rg<+HZN$|{GM z4XPokx#OX>Ie8^52v1B`lSk+P7AUk)>#<;{6`f1hNzY0QTHQzp`tCW~995DS-{@HC zT4!qd;F9_!1~t-M3bB%%6Bo&^4LZ? zJ>;Hi_!p#I(-pqwRs{xdU%dhREP2OMEEbGU$*rIM%3Y7mYUTq~K>zr$?4LF-B}__+ z<(F~0=+uJaM*fJB0~0)A7#g?gC17Z#e_VudKfFAy9CbdlC_6}X2p<=_@!XErQTt{7 zYJfNSC@5!=o~GrshWl+;G9De(yeM`~VcWX4kt;Z`590-BUP#tYU`f+uwku$cdrRBG z*79@m*SMr28CM1~Zx9h(w{r!ua5=n#o;Ssc*6V&VEqbc51*4DDAk$4xK_;u4!XY#{ zA3d08@)b$<$B7wUuI{w};})_fffWtS*_MRQ4;x zpvZxpo4P*2d9S!Mih9d@wA%jV24@8}N=*H!E@RF00lon?QsVgEFh~w;pGXBQ%k`~!ZT zlYgqv_@KF1>ab0IoQ$}rh9$+evP9T*sY~jTde{c}qf<&*ATe+w@!W-MCWCQE*$`!P zdA31R{PdU6?&{cuf`S4BZbr;(0;IBc^K(wT3mnY5GYNo!h?G)OBP_Pl$3I?WL#yXL ze-2XmE5&3kIiH*Adm14xXCm0Cjw#f6h)R@bq+2G8`x)_4C+;Gv``T*FymHgAD`KBe zijc7`9gk=vgWXG_t=mk?$QUa$>yD8v7mbeM)Go)YbKC4J)F|R{+n6>yD0);UC?+Ph zD_#u{I5;@iVDg;>%HQI1E;FzKiqAM(pbj87rRnyh;x~J+xF~roQR7g!b8h!b6U&2u zo?(YibeA8mEx3F3f4tLhibC1%ANuswn#JnRm|fgAz&G^jFf`rj>v2wn^{PcdzP0r2 z+kAUCX^MzT;lMr=kaUpM+I>Wwm;GP9qyrw$2&5?y3ql0Wz+o*_E358!Z+A#F^}?ibI)*Ui!c#t9ezE{b?MhHzJmnnLM#yajkNF*lffQC;tQ2 z?*d*-%gk8D8)wF%gh~{{@nJ;2zD9C?%DczN3QG$r zGn79_t;wq?D45=VtEmh%H8oEUa#lb~${#+}NJ?Y5kal)-*60(i(z|H7eosCaMUh_K zHL`Y`I<0dTeY09a%GM1}0v1gU;TphdTcHw~{5*{(n9Z_pnsb}R49pXS96H`&EoSjP zpSAD@jyXy6-nh>Ak0U9mB`HWiYtthssq-4c5bb~0R6R#qlfR!ly;6ZWYTFfmO}l;NYc z+DUzw+?@EGVs}ZU=k97UlrSh`p?-PP8o0T;1$|pv3JEs&5?lA@ute>}*jK6@j44kD ztVZXws7&#%XEYKMC$(q!P66H}-QVArHP!q4 z`Ez97r)RH0B1UMT%Ggza5*tQ9NVg6SzU_8RRnlD1zZ)tnr}0|zO66Jd2Rcqi{M~PB z8~B@Ow{N$d=I5I@cF#=1FQcw--5_*|?eDU85qj7EK2WWs5%tr_Geg@4U$m{iNhhsf z;_uY`gX1H0bhNmV0mHrO?s|xWgR=}ZYHF<0&qaAC^vxNuv9Ks^ECCnNu;6 zlSVO&5)42mv_r|rYyOGJ8xX*Fog4T4`}g~P(tP--rI_|eF_(k`ag#meN{nl?W@JT` zIC{~g^4+Nu?ojJh6W#zAY>$QG?zQ(LWK7?W9%^E-muYENU5YbrJ^8x7J_SOeED}Ad zZKDC*XG#TOq5R*yyAA`@o)QNtE0ImP6*Rp0wY4aqs~ z6B{DE0{!VuW1Rv~=G)BmKF>`EPj=5jLU=amoYq63~;$R`AY zQ)q5;XW8@gjq9Qkd@GrX~qw*G$(qv=QYo@d+=j z=@5#F;)u2d*|V|X1O7!fxC;d}bxez`PDOQf*DI-@GM^(?-3Cu`?xtftNHaL#VZdh) zqXL1Ef4CrwLz#4qqHp3RDQRfG5qheqC0hb(%g7*_Xy-YL>YFeYO93;F@UVPGe7&4` zzq2DourF*Q5D-LmggLVuD~S%kz&0FuEgvdQuUflOCv}aNg{mZm5a0EhKU*Fp3aTvW zj9pNN{!&lbGH6Mz3awXo3X(CoQ5**W#KPNvSGGaWHEItd&WD8>F9_VOu!!46 zv6&sq=TFu7m~gg|t1Ah~#Xy%v{Q`^BWzvj32^B~N)A%G?Y;0^rX0sbOz z5Xo}9`mzFV3(AMQ+W2dt_i~h_244%8X~MK7J(0Rk1C)(D2$(EWz!z?VUx4Z>nG87# zVl7G#%GXeRPCEg&O8xq;M}-^)EP-s*_r=9I@aKdIpHr>1uw1K(nCzz=E)-UdxHYfS z;PW*RJb}cj1sq93_5h?&zbnikQ6pe~{{|>Fh`R!K;v(G(;T)3sxEK#M3nx?l+(M(k zzuM<$|Bnv7qWs9-)wT2Ix8AExxLJWGMLmpmR_fR8B4Q=GW=M*(f)9L_nuP6IHnjMH?D3oR8BvTr5 zA$u;*czVKpIz%W`l_Do}Z#p~mLP`t1RD2L2eZp;ExGNFqq50~E&u~m}gKXHjIs!ZZ z45+JwV9d4zVJ`qVgxn450)wu#sC>e!`vn}HG+Bn^$)&v$H>a}Y9Srnfx>wZZ%Z)87 zX#jSQo;KBRW7QesUrM4gvNw96^*#zDeWN&$dYRnU@{ifwosjx5=+V&9UV1prXYG7& zRPaHGP6TBzar`yDJI!F3iUI2b`9z~(2d+{IPYdiH4u_FC zVEhxfqgbZuTvkKvnodUHINC0a&-wL#CY%N$mL2es@G>YTz~mJJlmqowGUK+CR!MRV z-H0{_Z7HzMjc>u~60{h?lsMT{i(iLCdXYxJ(l|^lj=&HXgF3L0>46@^vTA&h zqjD=4)OO5SpMyuR9n|XD?F|iCpgs(S!Vy-Ou~CgB4fXT1G$d9au?qF-b8!RbLpeAX zZiLu{z=7PcHc>%LNr{cx+SzG?`f6*^suUUAtzdSVH9j8ZTlVo&*=iW~=+3@#p}1}< zfxHSQqc>04HARCEPAga)9ouDO1Up~3 zxZBOvhXXe_4<{}3TdFfZSI3HmfoX*V;1|}En-q`0Gc~}a!E87Wq4-H`>;=#LwQz=; zo(KbGSHFz~yf1+RZn@*3t;)`U!;{GA*}j_>t#YnB)U46m+$oAh{I=zMFxe22<6gdu zV)XrmcCazi2CrrdC>x|S+-x{VXTB??SzJR?2jL0?mna4OH?s?8)3UA%$ugH&%Gt0U zx*LL|u{>7F2=0t|cwMl^7eFqZZzWY{86Cgm^m+KlP4Vnf*D1B$S{A5S1bFW>zer0X zu(R7_nS!+6{}hYkSuPER)mlQWd;%u-gId?zY;35wX5mdpW78PJ6x|5*3SY+V$ z;Ta$!L+6)b9wflFN;dhPAoiz*^$T>u{&8_cYm=3Dp#Mlh^y0OjlZ3EiSlI5QwKEvbS4H{5@-=|>rd4T4zBG{b@U_o; z()3baYS3om5uwEK7KVuP@~asxc@iMe)J`dHCz*0jTm%;a7&oF&JXwQxjy*0a9@STW z$Ivulip>15(g5>wlD$^&?MmO(%PfVh_*GlTl9h{PcvN7t_`fObmAVkd2lyS!;rz^s z6NobPGdg$c2g?rzRz3oDr$?bLCtBI^!`d-2A5hL!~m(B`{cD~15;D9pj~l)=0&z{ zaWQB8Rx9454Zyr+2zi6K%CtZS8LkZFAc)NOXk7^00P33#V1y@SiR;e70ho@5zUR6r60a6vsT{~JAw`UTt&~jQ zqx>kg6Xog!3Ly})>T3{hZQEB%n_{AAg$$eh(Q?vnpAUdKh}>rK-Ic0nP>1N~=_9R^ zKNu&Selrd;QYW{pZ00YzV8OU~XhbchF`*@DOJm9q7SNE^AlNlFIzHs05~s7r9wN); zl+R~0cP-Jv$&-6#pfj=e0_~!l;KK$&+(JN>hvR@Osa|uYD&$D(>e4|bdjdY?d2oNB z{8RbOkbwD3L=%#cIHWG`{RuN+uYFF$1PO8uBFm$Gn(66#tqa&3!N9*(j_I|5k@-`l z)dSFO>0$NXmXiyuQdUyJcio(g1`1>jf<*^nXNG6f@KWLv`$L{V8CLvAtF=gn0-hx9 zk6qwxgCTJ$S%F%4RQLUrqR&UeKgmWR4HTk7CIQnfE!5jxf#>j;k|lb&tUl)w!1z8p z(pv$StyOM}V`yYF*y!U$Cv10Kt#X;L9y&f3iUe!q97&@`bINK=uIun zHh3YH$iZ?GMQGId3ex~$35o&S=n9RF97?3igj&x6HC5{>TT1E_31ei%LIRy0gXM~) z>H(*EpupV%%TL2ABbZ2FfS}z4j;HQcYSx^2a9wqQ<2tLgCARIGE+5fQE6@b}BPW*yQwMekMzC5NLiv>XRPY7fB6jVIE==hJlo_>|%ZI8zx#=f0 zoP5KPRnKQn1DFur@wX&7{Y!k^mpo+>KYJZ#)S(!@?b2i+@JJ(F3Vjwl!$z>{wtoI> z1@Bbw1AQOz^{Hy1_RJ5#J^U|vk9D(dNA#|&16uHQWnsZHi1R-HA9xp1fbs9YHI3J-YW%m;q0Q{%au&f`=XH!)gmyw zusEOGt=LEJKJzewhmZH5;+&gl@{?$UfDQKQa~w&yw?gq-E=U~vUru*#Re`H;*vdUn z6H1oiil4uO)I2x-11I~AYT|F4VjHIadUfaMuLo+dAxrs-HVeyDv$&+uA7iDlNRb6( z&o4l~Kxh&aKddH1>Xo!Ksk>51uQelCu4bbB=6@ef++y$I>>oKD(83ju2IbC&@=$?C zXeeM5e~D$@L|^hQs^EC|w~6X8_r!~$B2c{hnfUAD6WD@1C3L z1uaTxNCEwO=|!k8KDCNN?R!Ace6ewyoxs-#xKS#ZUh{wA%7<@vo#X+X71K^8Kie7p zqPzf4#U^Lpvi9|UfJpd3)%XuH6C7Qrkt#9fuD$snO^?Lg7j;+JE}_}0UM*YM6!_EN z<2=xm`GAXNVFz>W*9w`YEUX9;ta?qk^CvA_$s(5&ndM2Q&A2-@Hk)o~X;INxGv#EK zHHKHb_3TfkS~w^a+){=OT$Ddk{8U=T7!%+9O&Ae+b;ZwjEnH46AMQhaTB$_%;RCg+ zAjMYarLM;tZ)@F^X9z%?X!}u}(R>-Zfr&C2O$^r?ZH6xZdnAm)qQWF4 zPCAD%%b|2WU`_w7zf7QENm>e6XDW!+98iV5rZ7g}$H;G!Z3{L!#lWMoWQ z{Hr5|l3SZO@MT7CLHOuqY>=hI_Ydg7Zo&5t?|#NJ_a<;Rho=k|Bi`jI9SS1}FOV8V z8h`C^^TQCHkbp4ULM0;I`>4LTM6$oO3NEIR4bfB=MDS;5p<#j_lXo z20mzIjdRHvEX1lBo&IC3PfzWgoe|mz z5}9f72Bm}3sxsZ4XvAmcym^w%K=_x#@Fu&hZ37O|S?6`1fD(Y=&{TEemVMq#IjsBm zA5F^AS}Wced!4{GJ~r@6Yk-~e{p%oK?lOo1D+=cE&NXr33HJXGEe>~Aggt(;dhRX< zfbKdEl!oUNn5&6Tjt+pXxoHnPsP|S2HeyBw!2ZrAhVDin(1wT({|oeX9_U9yN^fD; zwUj~qkKiIi9DCg-LJ!Qm21dZUK4fuR@mmDEz^eq#ZlTz~ctH#z)_z*P^qV4muYW+T zm2e+q3{H|=*hkC&NcKr2`dL`o5`#m0HimN)DV^8GiHkJffPOepehGMts9LA*6c!RD zJ!IZzCkIe56}wKQ@EUCaw4U#dzlidO(XsIJlR_<$UDXc=6zAT(dl32GDxLK7^~u15 zLr_C=b2}Jvlk}x&Q*FwI(NdyYvcDwzJ%!fis~X-r`+ohzQh4D>wa7yohL_zRTKXT~ z!$4VjSCTqbmq%w;s)7y;-l)Ta%?qgz1UFJb-oyrod}kiYxNr{T?pt+^Pe_RWApd7D z5ja+Foe#)d^_BXuhwVA_8yad$`V)CJx4i4WtoywCU>*I%DqYT6f%6~n#4)X!1?gDw z;{1_Y3B5G`v%PF}<~uy&c>;ow2?`2|PyiItnJSWOiO7?u2s;?Y3xbSurHUJ<)9!ws z>sMc-rb=KG{lz)Eo_q=X@A&I*FQ1#GE9I=RfhxT0bWX&W91ki*Ffx6U7Z=ArQYV0ba+LXYPeuzR$u3qiYL4l~ zt6Qen0L_c0zk`#b#M&*q8%s;Qxk&Zbf;Y2FIOF!JmA<=|N9+xo$?HJdn=_jIaTHKy zW@eh4j&rsNVB{^pIxxP$SFBxlRIjUg;QiEYa;b{kXt*0i>lI8NS2p(4+!G4U>N2L~^cUK^&3Bvlm#3%C>W`;K=RZ9?r6f~iZb~R5Ze(X0#jo?F7WXy1YVh=G0SBXF z>5?>wYutA-k#x-NRnaU{^P6|}t9YlbTp{ykO|qhULk_L-ubzEic5|vI;I)@IC9r&Z z7AXs_aXD@R?efe$mL*AiXrb;+YqL^>UN&+wRSlYd`5pUG|F?d}Nuk!SlQ5)P@&8-b zqeUSv$P^<3%Xcq4j{y_0XOR#QF?FD9{}wusxR{{?w4vabX9mIsF~fu7kQZ8vP`?YX z{^f*ZYGP@NAioM?djQbc3$W<5>RjL4-3!@X=*k8`E=9sm91veH!g(M)ataCxsYf~T ziR_zQW-&hW+vDG3=UboRbvNuUvk7;-k1@YqN(|1)$>H0M(1$QFG11-E_iiGU8u5CT z&G2^n#zkKc@$=uN@4U#*uO^DMAR2Z1t|nnQqjB=;P1Vn%t+<55@j1j4;DrLl2Qf#?z;)z`igoI@SVRND9G>!?6#XUXf6_;iW3E>AW*% zuCkuEUn2gf)=`$JDT}JPmg(slP5qN2m4`d^s39NpZiC-m)U z{_TX6{MPl#bMFV%8UeBP#nT(m4D!eC2<7`R9HiRJUW zvK4Z)@rZC6NCp4>w7>rYTFc=wh>y}r63jg^ZJ`86Z=AnBicZYEtbPB(hY#n^pZ5n# zC9t-^pfbJ4&+F7kcYsdSKvsF##?xO57fAQV?FoTd1NkEhTzWvk1Ek<~VYI3v>=DHp zbtOPGgTy`vbOd}_A`|oy=z!ya)|m%Cd`F$HsT<=LZ|g9z^fvpCNg{v;(dz%dr9Gu- zr#6mFVe^;JZwyFxvJ|+G=lNZsNl8f|aJ$DH5#%P8cHlAe=#p&HDM4eBu=n{6CjZ@K zRnS!>3)ESzUnkSn*6!FavKf<>?UaNDM#S?Li2rrFi;)rpw84qdELl8Wmmlg4>tIIX zgf)>_vHqrdmIM?z0Ubu=%=(2gXwg8EoNn+M!))NchC=*pm#W^5u1aQe8X5GD`OA5F zzDF@ZlX#?!(fXLQc4spM+=cJLz!r_9o*u*9m%;&{!l$i6JcAb8vRNO<<@aCvK>Z6E zBVma2<9CfTYO!HwXCWp;=vZwGiT*bjv_`k!?!VIUGP-|HpwSE-tQBVM+iiJ`lW- zS)fxdf6luA@7ExL>qQ!3LN!?*g9cq#Hs^715$P(@#;hNnr>l7qoPvCe;4W)^ekbAe zj@Efqgu^~mB%@tz3#RjVc}g!O+Z z;(~kS%9Wg|%2BJ#Vk3ifaPp;IFmoq(1x-I?)z#d5k*VqFdd8h{*$|C9r$mK?n|phE z&3y!NBcg)D;eB)fzhbhyq0MSlPl(^5QxX8!3u#?ObSty8K_LhvYqBO@cBV#tD~Dso`o$)PFU`)Iv-$rA@lb=CpMlDA+? zd4Y7cb%58n14!d@D5Ou=sUcXQ*6*wlipJN-9~2X?&>dhm?vOY;z8G8=LF^b7U> zd*I}Ah>gw`Tj23K^&YHoFoHot-o2%GN zpzG)T*%-2&%`}MR6hDFJY=&IwPn-%#`9BA+|6_#vzdgW-#|^s>w^An>UPudk<|cs^ z%zuy=|I-e?Ku3s})2gGOKOGAcW+%6xu&`akU+rnIJYn<$w^hOAIgZL_ZL!1?G}b2mQXX zlSyHzw94B*J#(EJ;WWQe{rUWW6tv)a`}kCD4uZ*HT&elRKT&Y8JKh~g%-)73w&)fx zY~DvJjf}(tZrSDs<{5U;RCJ%HVIto4llBr(sNnr#H+Pvh}TBwj}LLBKmGR7Aq z-Fo)mzdp0TkGA;n4m(){F1=cXzU)D1Pp<0=+#H3F*n{`2^6UO|qLecG(XGR_YBsiF z*1ttQ&Ha6}f4-~$X5*!e-~#ysbxT4!XXopNhK3Q=XFrNcp&`bweO9|vmz?LfU+ih8 z&Q=Xc4EWQCh&4Q*d_cGNi;K=81M5cTigjC)^{K@kZtiaF)KC8eQM<&$eb@@tQCtEWM_O9fI@$Nq*~W zCNsJm+zdLt!{+SG?2O263J-VpR}kRj8h$@TT zVuBhK2cK#HQ!wqdh8DQ+LRV}y*!U*9o*z$xEN2A%bkiS6Het9itxiR;uRh=NiWEB- zX{=fvZYIr2F6-7NWyWOSQ11}mztr=&mVuZiGU(Xjj7Gl2Rn?(wdqKYE%xa)lgU~PIn8XeU@9u%C;x5f8 z|4BPMVjbUufuyGJyzUe~6d@(eT^Eh}awfk00jP?jerNqAAzRBJIN|I)5`B7X=&}|Y=<75 zPjdKjxra6dT@qYHAje_9`X%WCS{#MPX76kH#JH3ctrED^G17^*;0!(R+V^QlNJ-;U zQo?q3oq^R(`!0U8#_!}D9wl==0KP47W#8k!JVbxh{GjBb>E!|f5Nf~&usg8yOyKf^ ze{FK#c!TK&q2=aA^-|25`(#v;)S>@|_ z2YXV*<{mZtKb^gIJlB2SH~v)_iL{WiQX2M1BBi9EC}m|UMJSYA$VikCA`zh?A$ufb z6=jbiGcyU<%Kkmy)p=d_{kZS@`r~&z&g0Q>w)lQN@9}y)*XxEvW3v8z5CrETXCjqB zH7oP!^(oBr@h7N0V^7@pstlnn_Mpn#UJ1d5&Z4JUf+7Z*^z&Qoo*@GWGzK;0^0|i0 zVpsD+)W$a@4ka6vzTtDa@AV<&_FwPiYzNI^>-4YN1-?%@qEgS7(4cT3_VRNn22^02 z;6sYa9wvDNltK_n@MGLT_4wmz2reJ^;^yZkYLK*aJ0gMulHO3B$j*mGAlfdsW(hja z^sy?!PKQtQXPfq3;mWQQaO**#ajxZtQXYY-(4 zAFSYinI|y9@oU_AX)5Y);xH;|c)5&zG^m^Ksxj3%t`JsusJ}kzb&`pZL;8fOy{Q8| zk-c}kQ*StJ)7H5RXrvB0YQMZjttZI%NkbEua?gm>?pc<#K_i$j{DaMp`RRi zv^9E9-e-&Z0|(^cO7Q8BGID|FeW6F#Yx83R@<-@UI#3D`h~o-HUl-V; zqhOJyI3dGa&##{7lApgH9f&K{avE+?(X}x51FMuUu&`i~l*|^3xvu=#Xq>GBtF;T{ zjM$V(f|uBy31QbPRsKWSTDck+u-MiUQg3(03}Tj?a;`$()+ zV{|Zapt-7{2^ZLBhoPRHXS$7SXK85kG@l%c_=ri4Sx z{r2tK7VIJfWps6QMPGe(8ZEUoi_UcaV1blc@wzQ0C>8(~gDxa@;bR zT&me-%V?OV7ayj$*!l}Eq~-Z|!;8~{GR8kCRe^{;+{b@*$j-az`TOtY-Itz@W%=wL1=I_D@~*i&pr$Gx51vU1jPfts*uvr6sANlNWEw z+^{E(Rh%h5YI=gl6C~^>M@4L!PVk2G({}NxjY-rK85MMq-H=E^zOkP|LHolj{S^?3 zw~Tq+Q)-v_<~AG?J%FoQG&|tCLYo&wBwW&5{!5!q-f+6OUy*YZZqBctUL;*8S%(m5ABk^igXCKRqK01ml8AcLl# z?)>@l6wct26O)tj@YRtBqTQyS=pZ?Y|77Ol>-!!Oq*^pS!%8 zksxY6&dKpwj&$ztjp;k|bF1euCMNIjW(kr;g-(?U##RwKjSmbAGgj*C4_@php8-eP z`#gsU2nFr1Ul0}`f9%-S9Xn{TocGv-2%IRAm7OHtdCp10{i<=+ZtbTSWxNIUp3$Rj zkc{7?^!ZNDlJCd66~9D+m@U1o_+1nZeLlHyE%T-Eflp|nJ8_x5Cz?DQ6Hn3DZ`@}q zKq7s-bso0{(Q8P=Rm#Umoit>Yc|XSHR1sWY=DSTnE`jCd==k~<8LbbWWHmVpgI!q@ zvEgcL2SLkZAJFE(YEjo=qp1wttz~~Thl*XO1hyVD02a@uJ>`dR@kgMo+_{qBT z)jkIW9hXziS=w*;#_lI~CHY(>!xNKloB--Hh4BGau?T>!U!Gz&qPK$P8?B8e6IUsK z#uT$AA$%B~?-dS$ulNo-Y|yLhf;#LA8|THK&?dJ51`l`JEi1%i>5lcIv@tFb+kaC# zJAk4W^d>6%)?$y%oXnc_r3NrFefXmIq<#%MyAKe9^-$_CAQH_JyV(G`ZMdGpvo2@8 z61A;PFcS52d&yY3^S+wa>a-74uH$WN?oZO=udSosebat91q`d`%huiEzes4tY-nHB zFWkp1juJGCDt&ojPp=;~8DOAx_{+w)hiCI?W?1Y|zctVMUh1+JmF4FqYo*oS2Y(=y>VCppHtya%4gD~ zR6-zl`O5v$tTlHHhO;zjd$_HX#b1%26EDSeqX-=0Bq|-eG@_RQV)d)XS|RYt4J9+f zkP-ZSASB5XL^XX}0L3p|k1zU4McXQ`%)Gz2bgs6uqzlr|OlT=I7Y45n&p7*VZ1j?$O0N*+6Al zmX-hk!4t8Wo!$EMD*$=FN6$xOH71QF#_eKGYJ)=7&g1$GU$wJUE%Lwa(NN;7I9VM0 z#F06BIVPW$iR(a~ocDe~-8Y}2BxUYz6kSH7rRYrl>qee~hO2cn}?YUfB7yR>M2KCGN^@xcAYC#V0Tu(@~$ik2nD{grk|C#32^V}j!T(ur%xHX?39r^-vWD{5Bo z?_VDf+I;dKiMh!t<~`87%LO=dS1SZ&+_$=Zy%RDNNKk0LLfIc#RAwJ64jngs;5cWnQgs_j<>_nQ>c>`cNnYSiIa2>pN}p3)eMp z@Edcd`V7<3EWkS@%~VQU6rrJ!9|A5kc99ebE*9WvtD&9R9ko|q7=06Dc<{Zpm9a+Hyx8}RfH9IpY6}Ky9fx3Mg@xx>D@uZcFTcZzlR{3l-oqwM#7oU zNO^R28W2+lfHL`Hz5Eoo*YOH^P>w(v`e?VwI_TxRD=QU=^oF90{*A~gKkkT>#~nL{ zt+RFieh#?tit&aL9^65TOJ`?iM@RuWDlB#Am^=R!-&DOCccsC^@3H96={}dYv^aO} zjrA_5b={c`{HC0iUD&-iKXDp1cftt^=Z!9V`s|sg^>^Z`+lao*BP1kbkMidoh}$FQ zH#CN+jx*PZR`@r*0OGX0;V9~CHjaFcMbP>4=W1hPV}P<IjpyH;4BpfkNKA_jRx2H-D zA-;w>{(HDtd1Q|_WKg+4Vzgvd67@=8xA$qpSNo!IM(D?DNWZihyo5%Pag zE;M=H!M5jg!tq%*OxP0{3Gy}+Vh_SR``&Bl#%6k?@-`Dqly$=1Dl zHky}e85Fy+Jy9MT?De0BcTB%!$ zGC?PJ?=Y}E&5u<0>La1%^U=A&bn(Un%mlzku1Gr_`g*qbq4MX054x2tr`5$5~ z>z!Yq-gYb6X5F%!MWA7!!~lQ}2*?_N9nLhYU&U1E7FFA(A-jz2Fpq zt`L69t5ImPFsrLEHR=_@Cg?3E6H4+!aKn4!!eaM+wh766HHx~*D zRweOw{`f&JDJl8<#S7L}PXLI~2+F`gz*t;dOpulSCpTB)n24E&Soz>!LH95`qchhwTqBcUv5r|>6rej33>-2d@VbBVzv^#O#+O- zt(l*mcReTXgPRI|3zRs^)6&v14h^HzH!wDS@nCiIee@w9L6WLYIwShGLUn%^l46J( znL@#>`abg3;$7(GFV8&)41C(Kh;%Ak37VimcAVNcseQPNegYO4dakT22TA z)VIviO#3g4^D)AhSOW6>4AM1;1%epC^78U5QI8P3{!_cC)E1YJ0}IVO$`SGg5Ku9TUcU`pz%#|z_`BN-l4E3(u#CjnKvS^mxK@dP< zo5`0T+Z?D2g+gdB(&xHwHz4<;;E+S1o(_!>VIj^ zDipxnkeG4Ke!Ir{Nv-k|-Z*XK%ltUjLiz(ZFdFE6Oj10{-_-2Nh<&U$;^jf-I{)sX z9X>V+Q145&1sA7`y$dceVvoT=R98LihYTq~mZHoh9vOCNYdVCfe96Bl`Liv@9ne@$ zPXCj`g#Eqn{+?LAAIgNMxEzsn>5%%FmmVL8v-CdKeNg!10s-&YR9$QfmDX3s2}by% z%reZ|H-QC#t5k68PGcXjiizTv}YH!*NY6Uo=Z; zcRwCf@xOidRs-fRbYhzk`v`G8;u(>O1feyoZ)}H21o7jveU~0$5dg@?qgxJlSjSnx0Lt{g9Xmyj6+wUjVB@iBvW{^UNwJMarDCG`Vxhm zwW*8G^s7NMXkU4Y_^q5>!6%-_#&%;%A*?SrCs!pAIQYA=G7>SmQR@10q^GdF>uvBR zMICv$L=#86UBoK@=Nt<*g78@;{|{OUls#`7S$++N*P1lykqH}d@>t79u^Vc0?JSVf zgrwA655dlxHYVPMuRSH4bwI?SFc-GutD1hIhNV%cJC7?UknCB(S9!H1Tqtu)Qtd2Q`F?c6RuFE2!9e$8kEA_C2M=xl}-)Fhues5kx%q2II+E}7l& z4r;#R9FOALr_+~eUgu23*3~GfdGb2P$NtG_7TWWLdfei5MRgCOo$4N_A>TJf_+r+A z_1O#)@T_}^dZqF}FO{GiCsCQ$F&i{Djly}frzPkA9rDn^tj;j}58OXU*{N)^Ls*a% z35M?KU$f+pp6GMkJYMUGzS9cuDFQ~Zs_Ta!SO0^CGZTO`!5dQl49PZP0Y)vAlAiAK z_Ca4-$WSJ~kJ9iSwcPy9ZU%%a8jZXBg5Z*df_1^_a4PUQPsI@1Sw}~i4*MQPpG^|< z6hfyAb>weGrCtm@45Rab=yinnaG|XD^ug-v<;Vv^V z(@oinc^tpzUT+0C1nQT#PPYdHXY>rm_l#&?bh?Z zYQ=jQP5)~E;_KA=&MzexA=3!B6+X#Y@k=Wa5rDTaNKeS|ik6eH8f~=4};n zj2aFv4<&yyj!Tq1*?5%B)yJf#cBV!ZDF~HqIG72H;Z~Ao zZbQV_jVAiYeRe{B*kvLE(b3TRRHFE}PASU*zb9q}esOPWG)V0d(XLTJ@5+6ff#*$m}rL?fOb!N^w zBGTuM`LZ7Em*tbgO30H+TIn%E&o&kF%Q3BeyAl0&qI0Qs1KzxLoHQ_(-Nx?WO7? zapTpnmczSb-igcsd9*t;M$}N-CM^UpW87mZ{i~2n~OK9@S z3Js&zMxEC4h$j&^c1PmmM0N!;6G0_9ew~RS%JHUKka8jH->6!XB|sC$?E@%Zfd^OKdw9A2YwT z*Owdd57fuq@h?yE%A#~!meKm&&+(x-$6_VtI(a$|E-rB$ci21Nysp7LHe_yZ*y*ov zaX6qW!=;GIc&mBaYbt1r$sGmaj*fw0x81M!pp1-+>w*_kVLBz)CSZ?*PYxl%`hHsu z-y!#K5ehPX8v#QT;P1pBLOL}xU)+exs%+=YS6qB-Gd|fhJ_mUN`_1J ze`HFJG1EZrVk0XB?zOJ(-_KplRou=$A$&QxL(ejC#H%eK$Nx4*uhf0N9h8I1*#AjD z_xP8r;+FM26j(0d*ZABZDOT~DzwGMnx?dedhiF18WvDDLxx+ZQpEeg9=jgPhpw)q& z*%o#s!-+D(N>?s9apbM0IAogo=Qfve-i#n>`wn^n> z*)IfrMxN^`NK8;KMWNDqeXWNEB=0pqqE>dV%F*ikX;z!zCWTnDTA*L(epD3qJgTs7 zKabv3)O$Ge&)qdz&YxV^u~=uEO<;a_CC_I}l>cXBg2dmE3F75ayf;@OtafJPP}h!$ zC6l!0cdJ(9s=fhrELssS3!fEGg6V4xgFjgGB&v0i-39cW2U4Z{y-O+M4MtDhUJ$Dp z)Fs}6nn(MrAdo;9kHO;n3<0H(fa!veL-vMPr4pc`8N<*!5q~M@NyLq@61y3sH5oO@ z`UT)*Il)-chDlni1|ULWDL#~!7o&{r?C+<=5<$}Rss30iU?}h2zwaXEOF}u56ixlN zdG8MB180Et&I8%?D5ph`_JJ4-g}FS~2cZIzk&!`wsRyK1A+rab4J-jgBh?cz7a+I= zfzWj(*`}ZQNRasT9Finhh8j6~A`dK0#$71DFmsEz#bB$t16dAo*X(sW;`Z(jHY z4HKOsvcrE5rPo+a4{0if?_G<{vgb!&>3>r#Kd+=3IvsnsZ7bz|JO`y#-Bg5k%L;Sj zJ`=Oy*}2ymmp>X>6|9cm6lBWWQgNW^0OwaNYK1_NzwL$`ufWoqwQ>g|CTa!rFD)ES zLsS&`95~jJtd`rJx3#rBmvx2hROA7Irx~Ctc!U(oQ;%_+BjJ<*U4{gr7IvWYI1iMX zsQ*d9NE#0!RsMs}eMKEphf|Z}^^?F36ldqIbT}i2|HiMMzQp*6q6)QD9nCf5R8U-w zc(Zh2i3)U<=v}kCYS<8JIzT!%bU0 zDNX4_7|Stq9KbT%Af-5uO+uM&X0XXQqd%k*)w!oZsTc(?Y*^_w?@tjOKk0i z4HqXTZvF4tzk5LG#D<}u+CuU2@$sRL(0HZl{dlG1`)aR+pCXVd2vV zP}PMAJv5i!DEh`D!TKlk>r75l-)*Zr)>QA#?r~Y^Z&{tS*;(rS6@mbL6hRX=dNsf{T3z-W+|R6ApVsLYs|p^RI99 zGLFBMQJvod4Pu(jJiiz^dvZ!7x6)||@sQZ6FD$QLUx&zn_4w@T3l^re@6I{G?OI$> zadOTcKflc7H(I33){Yy4f?eI+JfKmN+hN|=PJUvz2Xb|<9g&V_`@i6YGox&1I6%V< zI%+%eE&=`$d8~93RC`IV{Iej(fJKh3tEXox`pp+kdeO>W5GQy8JYb%?v3o)G#$=pr zE8ZtkRsl_HxlP7ttOtAF*gOKPofQg)rq(8JR$+QdH+Jdg1Gm0avQZr*7|zWOv>Jo# zR_N@}o$10n&(iuet^SwO47BaHkr_RG4SyGjy|9@cY|BXpQ+6d(zYIpycaJUJzO?ee z8AuO_c|_#h^7>y^MwhprX7TwSovI7-FfNYc-zgDpWPD<2gDql>inT-$j$MM z?_9ut((dd`J2j)5b$#8EOX^wbQ)prYClb3xdKUGyPAnQ4hvpw~LNWK{0)7#XmSpM*RXzVEzB%PK4qVoqhhOz^mWdacM^PkKW z3BuusB%{RrNobQGQc`|ZSoILa+VRt;o1|`aZSEm4PN=l=oydrzMWhq(NXqmfa|YTG z!c}N#g;k4ku(M+f&x^Ch`4Ypsp`;*KFCH@sK}rZ<3`kzZI1;>+t3t(BmO?`dWlNZ-s8C6skr=R8P_+x=iZ)u5*Gd$Sr{-DUw{_S3yT3yeiVgZ z)UBmO5^3Gt-Aw><+_jyk2Wu5X1sn~dCcpZYd1MF2R6AcA5e6PfMMWj;8hrGXR`BVC z2D=Wo)^e?)mr-tpQ6}2;8zJ}af?VX#*|Y0VEV+Zm06p9Tp!bfa=P^yaqpAH(lhKx= z`VX7b>IacdVL$jPdkv;8_0E@SMK_+S9Dlr1K!5?L@3*}Y0q+NQn+e0bE%3|NurD~` zMg7OtS65P2^%iJi4ny4fD`)zGvntc?teFuX^e}c&X%MS}EF<^6l#%0tJmWDVZCVTu*U{u>z|db*8d_t_44q4*`1 zh4&8^C&$J};&P?~2JEaO@gabEP7aYj4|~hl@1B`e5d_-8+isYS!97e)TFiimZSMzw z7aW!!RMB@s!w((xL0n%%Iv{`KNKr}2a$L$K{#y?7ISM)9GB|^B1CRWB?@BJ@Q0!ZM ziOh+>RyaD6uc5~HLeERUCB2d}Vv~LM?^{$wW4C^8va!t=9ZK5bKIrCoKKf4i+Dkg$ za-!gOZrM!Z;bQw0nO+&UydK>s)p&3(I(laJAuB@$whnM;W6)&v0L(mwYmEe3L&<4a zuN&_}-5M#lRfn1*OFGC=pjtWW|bBHoTAe=oSELxcpX-Zw&Ubb?CbXoikW z+4M8xNy9*wriO;}$^u1AF8?b5Mn)M1g4%{_Y2wS3St@&8?RA=Y6GNTr5-Z>KEPMK$ zS0_)IXrE8=8o3We}&#pEGJ1t}6qhv#w}rJ_W^1x`p45*i0L z+vHI5dZY+_4joAJA!sI9T!@W8uaSbF?hCT*h{!F1yG8`AN(MHu6_}-U3~VJyRv7jf zxjB;WiSoe*>@CV9!Uv3jE#k&>wRg@4A~68dMug8}cosZRCcG2)($Z zga|+gLx#UJe$;FPbTTsI+<{mBiJ!Z7PYx@Vm>d47JFq9-L!{`NC9R2K1*6i&plR6x zCJk`+Z@j)bAT(lJWc~_Ne5F9*v2B>>wve~Lz#;uWkT+R>vSr;S1Y)hn)Hy@HpKCO?878W$k6My+*cRVw)J&lfFS_H+ zE~)Nqmlxmqu2MRor~nA+~q&l+dC6%`-VCHVPY%CQ~@Y;Itq7yx>J@P%STXAh|Q z9%he$g`Oe#XY zQ!Q3{1hH_*W35A)!{cofm!{`oY^Nk@^=K(o%4$zVcXaWnw~MO0f~&ByHFb9PxnhzR zNe!K?6Sk2k_8{bKnzbJRnUfddA&X6yY{Lm38t~d}KB4pOD1NOKJP4{%FXkxO``x8wPTyU3e&+N2!fF zvz2ti1PJVHB;%3+-(zyfq#>MT9iM^@_gK>c~%rnY^)J(98#@ zL|QvS)In?E1^Sv>K)^?0gr8ebkP*EBztvnZ)ukhX`fu3@jYs;10mI;n2&RGddH3&Y zN}rd~L>%NAQH)!&gxBKQ`%o#cxY~e6TlMvY!x)^Ki??{8cu30BY@C=XX-n_RnWO^%fElYNFilOC!0Z7Yea?x0XS+Ns~k zjJby1ly4--LZk@7EmsG6WB27+3MsHr1;kG5G6OV@eB?B+v{t}FFE_Z&&|wi5fWNW_6}4bcc3l1Ff-oAJ3T`< z8Egai&WN~%5Vw#Pwe+j;F+)lQtq&E3Ehs*b$@z3TfA<3JwS7>s(f~?<0J{?kTj^mk z@(Z`yVWAKb(o0%y>nFmG>Mj48G>9h>FBe{$LlF06i1$O%LwbENF)?n1bOX?ZukKR}hxIw3r z|D}idlu7OLd&DORyQgbN2y2H!KL=j%)yIe)z(-(1N#+iH1o;yv8R_#mRYYbO$k?CzLB!$Ppt0$B1SJiF(~}fDIH!(UHYCh9E6L>xb6VoXktv z&zs4-$RZ{Sn#)`_|D`2Ne~>X6)~9UiUy@Ziz?Qzct)$W*R$P@?jxN33MYD0-B8)8; zCFL{V%)psGwN^u>hhEhkP&8?AQVq*4jQ4)T)Tx*(B_J+DgG&@4C}he|`iflqum&5L z=w4r2b}b_-c?C7~dE{BOEI^9uMHDuGZV3fSC^b;y$U=U8To#5!vanFSEeBue0#Y5= zs|U8-I$TI@IznYcb_-1nndUV5>lgXIDOX$fTopJI4&bN~Nm?jCFgl|QZSEM}b2nr| z6qkQs4gqIor-!`}+hhRiT%3Nv0RTn1Lx1obCfWHlqzXc9GX^}T3$+zt@d(9(hN>I% zPRmFc+5&>A5flo|c?qJS|IDPB(+jujaG)h4Q;@TzqpJ&JfY!x3(-@&whNVizA(2QA zGOKS4O9uM*-~o0t#3Vcth|>LnrNzxC$d90J5Jey_r4YR2HNtVA@*|`M(1|lUuoqY!x2T{J-b#XFxksJ_#*LYZZ}TwWfb=gzV2u_>ZK3$ z8sXx1K7C0M3-+`Efd~wQe4~ka`J3=CT)TG79y&Ce zWqKZR(GgT20uL+^vT}{3$4r2nvY2 zZNdnee$6JzGPL!81TlVh1Bp*ZRXU1|v5Z1kkM`V?O9!x-h;>8vjWrd89GoEN-w_Xs z3~cSc(RYYmct=OZpHW~EBr_DdOPFLKBZ}B<5@?$x11%RSlVWUEUo1)D(f8k`u;Y&> zf^-w`{!l|LL5@9gFJ~^z64&84Z%oX!)1_9&xUva}=$^|Zo*ac_gDlCo<`WFGcp8I3 zx-8unuj4UhWUIuU$}9asOiI{kMIOlp!A*pRlX_u#8^$WZ zvlxYEqJ6FY2GYGmE#<2zP@V z4?AqdzT9~x#{ww0+#?3T&mG~+zFa3~(TaNE0?=I&qznLMDhp!<2~bm)ZhV^J0(j=` z`W>~nf!56-RgMf5B)gm9LSlPSptQUt<7VNwAOt+a=Q=Uy4-O$Lf3u8Vm9yYKyO zIoN!!r9!C>Q!c)7&qIDDSGF(RRZ>#=eDj(uehC@Zu|HRz304ih)uoQR7_@#6k9h0s z-$W4tJJmoY^bb;@GX$Sn#iKaNsfVSCAe);2{N;3Hywp^+5k;K zTe0J2e1}FQ<;Ik$`&8He)u!{Qbvjr>uZZY8z-j%Xi~ggJLKnsbuU#1=Q=-r>5*0C# zF`;_H-qSFZh4&f7k|$uDL+D2kk9q{RcqY{-K=u2qdiGT0#D`W1hpDUq;1}FE79RS! zs0)bh3EoXY%My7Llu$xCuU#NED~8Dpb&;FatE1>5DxBsK2oj}V;7nxEkf3{`!e?Gt zYiMYQ`i_hPBR&*pWp;nz;edFngdkB(dN&zKt~&09j9u|iOCExjiSat;VX+wtMRW-$ zBA*?1vw{7Uo&Cxn>)rst@@kbsl$|H(;zR40<$jgs)#J%iRnR$mtg$zrS{o7bM39?i z2R}+f3G0D+Y9IxrkatHb?7a$|=-K z(qtS8n82VfM3t@>Dn?DpUlN^&a*2>t4F@n8*g!z{%?2Q1^1x=GjPin~DH^+w*?E{$ zA;{tqfh?IQ+;RJSZ_O4aDmYL+K-ivoYsh>tf0(O5o1HdL^21Qexr4 z_zjJja{dh)%FNUw z^>t78q?(fLle7Iv(^;Atqhm2k?iwo%mD_xhV(C2=ncJcy3p7h~9I7>QPF$s|rVz|D zK~lO4Y;i_&Bz2+42<;tv3n7#=t4WQDd!iU;duLg;I=s4%M9#aNVy3e?SKz!K1O1d^ zLk-LVf`V@2#OT4or_mg)+MqaQWi-C)Gp=YdZA7|nFA?s7k!9UAsVLuAq-k^?Zl`PW z1#HzAa`7SH9+TzmTCSmoyau0pB?nhI|Ana8AS&9v>V|}CZYfMmU4482ly`re!9*Yi zDhpr~Bn6)cd$~wTG72|C65{}o+ZEe~?jKrAPCRI@ExIq<_&=ZkpZxAe0-cjAyNsK zd5>K%>-5q-jR{fZliB))%zNVCpH^2B`Hk5}LTUE226?+_|IoSvE8CFrIK}^ZTZ)t~ zut!)4MC>FLA@1E0? z@?TgOJnt>LJu&DqZ6YW*jN{Co>DCyxjaR-&z;ee#rw?1e*rV?L3b;=R--Pp=NGrcI zHy3quaALgMBj|j9gI+_No{HUFugZfPybPy;hs7FwE^9Whmur_Rsu*W?GGuPQY!{$? zRM3D=wkn?&i4tJ__E=eXB|b2^d&#h);u269%#sd?NI*O@(cqw@c!m~~*rw1=%NvQ6 zj;PI#e$}69cHoOJXi^n_${QoKVYRdODlG|D^R9bqkhvL_v1tXHCoWYe3Xrw1n!6yD zf60D}7z5&hF(ec}!^Cu-r((nAKwvGXa(jU55gv-PfgsTi3WXTr96ZAEjGJe$TOi%@ zR)i^;2XssNW>2&nW6m0T=c%<*dVLG18%A@@{6zTNb8S%JpPFy?L=YO2k*y-b%bQMWJk{^ZP zzLH7*)Fqe*UF5f)2nssnCue8dRL{qTyjG+l3@2J#^U1NNCQ-K^D*{6+z>wC#!UAkT zH8eBkZlW$UfC~k@l|NaIi4SVGg~Uuh_VOBUMrsPw_^7uy3B;6>D|hk1zdZQ#S2f3E z&6Z^QJK4!f@w#&R$n)KgS2Jg*p)V%`3;08KWLlTS?g(cYZI_Jd3f| zs~@klzboMT;iGGd*O4s2D!@&h{x9A=ea>`-QQs_E|Jn`eGx;5dFD8c?4ROHkCr*?1 z9^(xDYy7aU1ii=8;!7sG=bEo;Zr0{nex}prtJ8v}`1CuOqsuTk+oxmkewNZOr>jZ( ziS^?{pm?pMfI4aZ;AfY$5<{@7fC6%>^tl#AWwbl{Eb>`E4zx`-{}Zd zLoOWg4nx&^rt6mxPnxsZZ1z?B#2F~WpRu>r)vZM=%aN>#i)km8u8X}BjhDu=LqsfM z%&UCKm_eTVm2&?_w4SK=mpxYYR~L+_;@bYDg{uy@WXlNYzx4sG)(P*s@=MjWFZLw$Xq7)CUJj17;$MRI3BIzvwd7kwHDQ!=U_HbonaAuh2m0b55 zFsjPZ8Z2!V)2@o1IFxaBTX5>m8mUIU>>l+sW>n zr{C&enKY5R_v=J+(@bCXp!Dsmy#^cXi%sV!(KN9yY5x0bhpXILT3Vml+7c6B*J;sR zDYh}6ZMVKzqo2dRjQ8nBaI6nk8h9IfFPhJ4S2cyvPK}s7XM5!|YH{jfZJqem-#=t1 zB?j{PCHunhd=lYSQx2h`&^?WfJ&M|Vl?@xEc*AlFuFUN0K?q?!gIS@OH_h&?mM zr9&&mz5Fi2HZgyi$U{0h;qIR)DQgQ>FQ-rnO&Rhi6iUjb7S@l@&jz8+KAtC@!Bj+H*`cYFq3Q6jZ`~AyY-6)VfwO z&>Np;ZPpws39 z>gQxPw`Id^IV!FP55na22F#$Et}ci50@M=E(TVPol+?g4fuyd*HKTU+tXO66zJvlP z>$t>3)vH&p_M?3^e8gv~uA=hb=j)!;`G|IQ(y}n*nv`^3oi*t)S=0Fjb`>Y6VdCTB zl+DZ%PzQX%KRr@eQ^g0xz8k0rQ|Q*y_@SPC>wpx6T=t6caxqLtKh%U~sQu!lOEyx| z82s}H#P4cN-;(G8Df@iAtsmz>0hTDYj|N^maq^_a>(a&p-)ln{uKaXeJvB9TG;nNg zVSiF;KI?2?(dF;dl<1<>$;(L^9ikWb5t~sl26WSD>a)h7ni;e=xv%tr8WPkhGb4jR z^VMM!ZYk?gdl_U$)|2pwsT-bYl_?htxfp_~_K)JkJ`uQGDeP0vrq-L$!fe(lIjof1 zG|Q8hZ`!o!Q)6Q+P+p<3_3PHDYG|-dhNA|G8r0U-{#;utaxrstjBBrdP|$OrbH{jj zd8HxO0k@e6n^=U5!}YxJ>LS^Vf3UP=^E`z_@pOl`%IveIXoKNAlF1_=H z^7yq*l2DDWXuIE3)`2f$0kN`D(ChGti>FV_0D)9hQF%saExUG`CjUk@p)14gZ`1N= z_n5cE!8QK_0Wm{1uU_TXHZfco!P6&tY<^sDTX;V5Tqw3mRP{QxtLsEJhpGi)U(#se+MIIAr{~@T=t4V#uN)vcUWNSXY!d?G%A>&Xaa;;}$Ga>nbAi^!^ z>FF!cN-C+T1&fwGDlV3r_^|c9brN)Jfq1Y$Y?3?_%#L_3Fn8Jv+=ow7F^&uLLvBHe0=;H-FiUUhu)W!4I*OotdUVH#PS(%t4K&l3;=65 z0e}NtZ>CAZ_RP#oo5gwP(aiuvzv+9Zy&F}&;JK882O^W3-T=ahh5L#R|9l^W1}QK0 zbYv0?hd4vc00NwPM@4bj`lXAy_yZa+8yMNDLdB0kZufATQmEJwH8r*N#yygfgU~4{ zU%QsNY4hex;2p2@^0m;813-F>iE7Vjxg3>Y7}7!{?Y4o`GENn&!F%4`!QtVF_4Q}l zaZO-aS~z%|8;J>tiHa~)ubgcO*m^7juo#}&Ag%1? zt-(9)6c93@9F0RXbM<+RK-5q{VPWxKzI+iX`!P7ktAOco5k8{8E+0Zsx{nJN7^|?} z(3n$sd3gq?3Zn4+KFG9x(jTTE;TQ~(fz?jK#N?%xwl>FX5QW0Pxi7Dfj+u?Exi6=p zkN!qte!gztr;d&!=z${+IBCIisN1-3^Y~(NTAEn0@0?;f)!NAm3KjlOT+rU$}79(^2~G6g3mse|5c zZ(n~uzpSi2I2vVj^@nH*UI2iUJ_CoWdvW2a+o4Fp0j+rgZ~vaJ@9@}Jw?* z2kw0;Ie$7#hVwqoee3RbE8A^buO3RS6BHKKz*Ls{1`NW;0rsG(t^Jq?__6LeAh*JC z)&O*s{pDtgOHubGJ?=_4E0ypm$?F41^Yq6L(=unp71R-A>(97z;~Bd6q(bnS%T!*$LD?D`+fJ$t(uytsi~pLKkPnz!Y}N-_F8M7h|9VcNr@SWQ79CtrUupk zg*w%MLg7pi;=?BwS)1Sk)OmL`6Zb35H{I`Bx!IvES-HD7IJ-OCwr2OVbGvuj+3B3H zq_CJE`z?2OmwU1zB98w#K-k&MUS!y6vk^{0ehL~McLI@r$?elaP(_sW@>hJs`ja%oAXV%wyDpA z_JxwQggQ@;^@NCbO5DGaV5UE4HGh1Z#lS$nlUi5dH&H?-l6qmc@4@PZxLmq--}xZk zNK82LSD5hpD-H!3{urpJ%rWp+en-bE0>0^fBR0*2d{EF8oc#At#{b6$JZt=lV}hx= z_Bc)2x$WCGnjm%!7BY0~M@ltx46D3K(X}c|`*#Y=n1L7~=hI^i-c(IZO$K4gSnNri z|B>v^ulZ*^=&`nWDeFdo1qA|uhtej}(lu^uC5HTEcZY<7!@^XBlbK}YqAY(oTK*a$ zFC|5dIy^d0FyEqE+J0cQMpt)SN3ftG^hT`e%UAJca~~gBzXuPh)_SnmebMP6)Fp;T zX=&F(sLbu{?Q84mMtVi++8i259gg;Ks&OpS9H-1W8T2FcUEEze63DJp4xRK*$E;76 zk)uZ?t5wzt1Vzqf$BDwA#f9@mmR^}x>Fo=;mdN8FqZXbWF8K0$R>}Dn-|)gs@AQty z6L_b3-%1*%uGsmcvfOIY;rX~TTKF7^j{*i0NF;)e<;3b^_bUl2&)>)29Tbx6JCCwG zL-XrA?P<$|le;o&KOBd%P$-V|w)-Z@2`9Xl6v)fuQvhZzO%gJg0$^yp(r?vSfkfZ0R6=N$d?TFB}C~3A&vidDe&+ie&0{xyj+t zp>W%pZDYTTNng6K6y}9`D9`wjRNsxCJX62LDZ%zTAePWCHpRB|H^E-*biMwI*SDH} zawap#jXc(lRNX2O3twe@Y_wGLbU7*dbq6oXazGoa-U=&Er;mEOATO`vN7kS6APU1) ze0OKFrhA5*-jeEV*BD$iUBk7aRUC#)be2R zo9{Rg^VB33C13r1qit{TzSbq%k3!x>go&>Q`ns|V`Kv}HHH)X7rM2*;-Eb6G-(}Fnp2B8(wgj-jC2=gdL+{5R`Lv znfZAxnd(LVpo0B1->FW$7!H0_SA+wam5s4AMca{5+%Nd*hd{}TbBg4#ws}_sp zl!tq@y(*z8Ol5|_3|PXsjb0Sa5kJEYnYQ93hj$wxLlAW}X~Jyh2PGN}8@QZz?vh*T z)W7~iXvb%2S$7JJUTTM-?xpp;7ZKh~k+M!1JHl5aovbO0Mkf-$uf$XTv!o1v)D3sY z{P+=(GI;x1euS_#ZFu-=IpinrVa;Vz=2&K?g;}AnsMI?C-tXs_GS`ZUL;g@VO*CFv zpx>FNMC;qZlf?!8_btMGu5p}mcwyd3d4`#!`b<0T$&icxgcVBY1#hLu-@CdDmq<mBr`BH7S{>oFeH z3)A=m%seU)?@YY5aBjM%a_9%cFIM0#G&Nk@gP-Tbf3>1FYu&q-dBc?=dChiFE>pm) zquYG<^M1dvhNreVY%Y}O5`@s4(s)^vs!5sk=~JgpO)V~lPfb}e%eZ6>KcJ$b;viIs zEona9?+HdznPwi=-TVDCHa^Tgi4}YKtFA=EME)gD+&A@8kIkEBpUtvI|2(2#bALvL zN@0~^DZ62VPNWa1&6VL3d5DB^TloeC28KV2IWeX7pICEWzEmzU`Xv{zH1JZqv9Ief zlbHXZSD{JSDV7VK9j`L|BNZO(vvYBs_V_g@v{QR;p|ADh$4dvb>W>*|>M1Zm*Mw$9 z>8M&fI6%?22f>zvwN7o+JP=H_xX9Pj^H z-`L1sS4s>yLpM`YGtm}~K8r#{(TQ~RJ-<-qHmB8_dhXTmY(qoC)at7Fi+hy%3LhhH zeG(S9R-NVC6_Qdc*4aec!vN{W1Wp@pyl3w)*~9`jT)2Ms$NcJ0Z)<4tS?ePd_48P^xhnj~tig;UPGG{`~iMOG|r=CR#5IUM0UNlz3HIL{q61J>pMGGbloRrV`HQB z&D8=5X6EShbml^fW>$HhEhTGfjO_i@!rhef_vS6FevGd7{X{@uKbB zwKLZpa4*;ie?QzknrQURN}@A&YxRr3JT1UaN>7?r&s%*ZUiDlpx*+pOu6+H6?Pt|q zm?--5z*qe0=ge= z-gPlG_|;?h)}FMhq~7?bskf^HyuE(c%7e}cQ?K@)4(yV=v9S^^HD`|gn6!pNdfgdU z)hqxq>l@>eCt>7lMvF%&;SP^rX`Z1KZ0U@mn_gKV4G1{FID7KwWe9Fd!f|A&I!3wD z(n3N-8YRw}-)o#>N}BfH3@@-SGfz!TUAC<^6E68GlK+9#nbe_-lvtQcFnKg>dplWz z{^MoslkYR~ms5&rl78Yd^`)ZRkokdvN8>ht2xhmBUJ5`+KJ>|sW;;lywy0*5Q<^e|@!NI}o5)#w} zMn!Efj-q$I@}gS4=3jB0?IQKtn@dJ*+Pkjr?p}=NzVtLLEedX%I8Dm2E$>nq2_<8!VBsvnciZZ^YKiRHAoy#h;E#&<^$Pg&{mWr}N6cdi!C5|Fb-*WX{mNJ-J= z=jY%0UdiXVJdC5LsEAU!bBDk9Mt$D6qO>#}T&3+Xll0V&AHm$0o>?C3tSpY!lqi=( zbsHqSyohYm;SsU-@Ekpyrfq3SNukLW{tOWj5ia-dhw(;w{aNRLRS`nnCl=<5#2;EiT=<@#`$O>9QSlXLbE zb{lOw8q?aR>Ptg(h$_Ok2C_%?| z#utX|!o9Svj@S3sy4tK%XJxVbZNBFrBqvYi7sM1?HHay0y4sK6(GXjrl50hS7oJC( z35Q)PN@B-geq9~2l<2Ef!eXnH%7bfscUS49otT@BcCLSYbM*|$1?EC0MHB~i+Ee6Y z`U92{+;3sDwIPBeiTK00WhoMZzmNr8tmRD46V;^s#!GrXX1E#vQ7);h>jCTS$C z@9*!={CFR)p`p=Y5pZ-`pBha~9h@QWt3^W@t2Vo34bQr-sX*>rV&3?pP7ZF z>{fra^)40OLn0A?oN;1!*hJ@<1QA3E+0lyf za#80AIjowRCN)Fso>dIwX8lfNb|9o~{MnerjGjApPVC;i4iPM5y*pLmtY_kD@kHUM z)0%_r0+HDh7M}wSHwYj;aRa2D&#B!!y|V~FATjejl{q;%Ibje=nc{PnGVXJWVTf!o zY>Hj4^>cJsXk$ls^Zl&!Z0QF79v=UmY63R0DzzF3n*uW|3M_=gs!x2>#D5MB&Pqsp z{c43yJ$-5K6hSUhS3H5kr6>KUP!tptfn+SQO2Wx$vUnKH4$dwm?ru@Y`_EUhc`-g~ zVV5KLnU5RK_0YjWAnDB|a)vN6Ha3PrL1Ca`WJFtDUcR+jFcQ;!?Y|(66UQr2m>|VN z)?L=BL+74&+vK@3Fjb*e1An76?5Fk3&7k`F`UzHnkCsx2T6P~kcXwd*gP)7jK~Vep zMlQcyEMjSeyH!D&{0!y7lpP02HY!*|OUWl`^{bzt1*|}E!FcWlSa;w0T1uTK6|Q7F z>jjAaKafaVa!R$-KS;z$d18<0_#p7n-@E9t83?Y?P8RlQ<{8EO2a}wa3~b$==<*;K zzrFi+{%y_TKDHn9h%{$uCGr!VIfIKrcE_LPicb`<2K5y7@K{+{4MNnp{|8pNwYl)+ zDonxF!4`e-WY5~b$9B@(^SW5p1o{bU+2v(fO=dncI(9owaQeYo;RtG8@b{taH+c>x0*ZS}q?pmwsvmZaS}S`^@{=Q=CPM;wIe zT@+W%)_}!Xm}X0~HrJC3I|L6kTzQ)cl1Ui3;vofubnIl)gO|inuMHxvb6h%WTpO<6 z$Yy=#-4etQg1oRxVZFiuJNSW`B5-qJu52iY9zVav@(X6W@fBUHnF_2NkIz_J7J2+a@!x4${pBj&Sp%kSi9*QCyk z>&{RyDe4d8iH)r>*y$}-3=_4s9!F7Hbl#GVD&VR5oq(N;V)!3s_D4}|ynS3nIHjE0K?I{IAW zY%x~riVNX5m^V@;;8+f7(Xs~zo^sn?ufSaAUNy)*Q;NioZb}#@bu4x&|5ZjCgzc&x z(#b|&x{}5nM!11e_({IXS$?s8(oNU)`#CBjNJO#afZ!J&7v~;#1GT=j)p3#SLGTln z3tBX^Bc(yxbzEF;`-l+;j7`DhFqXFsZzS?s~!!1(!#)#GbtzMEPYqO*b5*5BkCtG#T006}*BXF-V;HBIW-OF?NT^6^lcVEEbI6BMlg_FEaP zAwV(9`%n*+SiEC$Zg>d0a>YL+-nv{zEJExlk+_btbbj|esBR%9cDO&rxx8a;7KoH% z=tQ0J;N7jIFSo|uN}{G_I-^j5jFNY{og!a*GVAa(7cC(w{yUlOqu*l^5nC z(?7@-&G{l|J3s#y9$+3pR2Z*u&NO_*9RZsf7e!ISgRk0;GZMsHK`yK`R>GE(co^_U zyp_R%d-tAjw)P)mJLLhO-~Tnt+WU<;3yWkYBZ9*R7MIJm|>m#uOECj?l?Y%z=4kC7|~=2gM!XBCdNuE10f@bU4zzFy-LdjvT=y6M4z^^fk@KQR3+i=R27S><7V zzP4m3KjN%C)pI#1E@S{dH6Kf3UEeixrJ#T8X3HJL51lXwJsbJ zKEH&b>BUZ=O1(C$m&fX;1dNM2($C+ETxMfqivcKSy)n~?3WUOL7Is65Vt@jbx%>x% z@+?jF=QLbHQ+H?+FWcMGct4pVK~5`3`+R-=e=VU**2FG8VOI9?s_m-=aA&jq^V?QO@jufJh3Zg`fe{M}yI`L=ATi3|5vm0lXOj*ecpudk_*b{c>CayJDYera4$ zM1=hQ>e$1W7`<2-Z~J1BM>w5VO(eB z-s8J(XGYPE#GG{)6uXE>iQfXR%ea;uE8aN=0-JLSzAc8s_zqA~G#6E~pc7-%cp4Z( zeCgEm1o+OY+i+hZ|2+axK zeO8cFS|0-i6#oO(|5|Jb@E<)(`Y^VN!&gc=jGtI{+}qnba36z7v+pNzhT-6de;>Iu zpZD@5%~I#L*QOPD6Gtm6E7MC$`MW9kS6*Sn#SIQ*~G0}8Qj)`&k5fTR*& zMNW|yykj}OIiE#hpj0Z`e!f#$an9_0P|EWV-(N|mP;c+A;e-?vw3?3^qVvn zZJ`({+3R>5{+bh}3=$+B#2XF{oKK1;4W7O#DyQ~$*GhI%Ai5nwu96+~T=^dI^yyRU zNtgenV6rgn{I-a1c)zu}%u2oB)765E1f}k|9jU!2bJd@|EzJKsu>W-@X$=2`sLU{F zadAoj3Fn*s%$z}z85Orq%fIWMy~YaXhK*3q6a5_Z>7U;U!lR=*Mk?$#I_WGdVU$uN zZpRrWTN<2R_`Fgks=0Pp=ZmRt>1<`~BlaazKMT<>%D!K|0VV3SZaD_&tE{X{?a@`t)Mn~^cTIN zQ>h9xyo4fNQ*MYZar`)#^IF`b$QYO7=qAZ@NUF5N`-{H2lSHQ_Uj#^>M4g!xW?#r; zmT6~rMVAq(%0lqe&d(F7?jUHzMvaQpC5vZhWT5or^!D@&XIMpB)3C^0O;2P==tK~eM0}+;!M)Vv zd@Afand~UbPZ`5oTHsQpT zp_2BGA3qYJpD6-ea=qC<&=FNiO5%Ey%{zmf;c5Zx^0LXxi`f0Lu$oiL%gdKv=|0zL zev+1!cIh}ypnGt%Ww3@lnXHGsAVl9t|Qt>3&ld_P^D4B;#lVjM&-PQBc-GTJ3s>OQ1ET$j3+2 z+S-bwT}w;$nc3M()cG3;uShBX8=J}02X%LUiG?CkqzAEE{u(TL$!g=ctK>b}@_qFo zP+?h@(xhBGJVKI_>F`PD1A%}{^hvK8BcQ4Czgtfb-t7l_FbMb#>%%`ADGCo{7hiyZ`)31dL9Kl;dAKeESPG)kPba7qqI$*TufoBbb9 zsX3ScI|{Y;=_C;g^Sq;?4OU)frduXG@=p^Ig6~g0pt~5uss`b8b90k|mGwz8bwW~jtUXcvV}MaPm9Lk%zkRk3?D~lh>nIxg>T4V1R*B%Ca7{DL&BM;B!g{%^R>d4K_wcah8Kgbds za$bQeX@yDu0<_(1%>k?+*o1RaO1_5+znef3HDQ@^Jj$B<6(d zXk%leD=@*0@5bJGXs;c6dwT;X{N!KfkBK2SD!LW|GiLRB{^PCTQVyT3#fL`+yJ;BP z_oi?q^xx+R2OJw567u5qm%PqZ`=VWC(~`+&YdDy=Bt-cWP^NHajmkE$y?*=P*Qk@` zhcEb0{b1WeNvmR1RaIS&_C0zt71^F&xG!}leZCC_YO{3S*RnT7Ovd9UH5sd%$NAvcZraNmeyL6O{0JnCPFN|(cU!x=bjHPIw`amhOXM6824Vv$SV|Ta z7Byg-!~_L(#+n|~9dW`Uf8H?rE5JfVgZ#_;p~6;y}h-N9RbHwI?N*%8@- zS?1_ZJHmk+2&mPKjj8Wp(y zp1)N4_N@v9UVb|Vrrh+uhVtPsRqVdlbWtE8dk+m6{}c@mUsQzB@8WY=k6f}x;l0#U z6T}b-Pd`1u|202vv^txp#f88+P&uH0hE;|Bj}F};CEBt5>eZ`uIMG3q4mOt@e)~B= z3mFJH2+&kJrf<-kSR@?HJDfJJ+GjwqWo#Xs!1FJ39%yu1NbJpTpmhJWe4b6B|$|VC0(w?*Y)_X93mqEb=pxlwjm0N8LiA=)bH8FCP~yrZDfbVhKV# zcS2|4%#|tN0pKofStQ$$!D4?oE7yR<=LX%_ZPPt&Jn*NmSJeU$A{<_k)$1P#P)}wE zUtv50%J;6D+VBX`FxALuWrep<8Mi(#f4$T9cNi&ry&OSmmBr1K_|xyzng7i?)@KyX zbn@-H=f_E&3kBV-o!iT_VSg`Z2ar&GNxS#&p^E}mqBpOX_ktez+QDSN@)5S|k1d_o zX1|v@S#q6FLEC#LB@epv)Dfo9vpVq(Nk;P^cfPZW0uKHrg!3+NO4KvdeYXYKBx+~Y z`N6%W&_CXXHN8T;5MOW8`W zdkhS|#Kt{)0;|i6*{lB6E&QX~%qLkqayO`NCV!@39jQ|kosEZdbLRmQ5@({OkH7zBDP?;jq?`V6-f;)P6i`f585nPn- zJQW_!vrZc zT{1O6;%gP1A^MyyRhY?lK(o*Zn}yd*_?$Vb9<4On^|%c9k3akS)^i^cxmXwG=0cuH z-o4XoEB@;+ylTCWL|MVf?pP+Zu|e-v9*aOq<>|dzOP`m)mApmaZ;Sx{+QZ0`y&M2m zJ$?TCF``$kw-PK47Nv=)CI;W6eHesW@ZmzI$+Y=|;7hI13%AZhrg;|TGA_C``g=-! zb*tl0k@S6nvX)hE)89rLEuUW2XOb2_-K(=t$j16a-C=0htwiP`L@Zj`=XL*)lK(3T z`}60IC8T0N-Rio!WP>FZz|i`zi;1BD9u4~Hz-K_QRK|Sy^4oVWL5nLv%dT#Akoiii zP0!hL(t122q{Y9;>av7)y_>xS@PzDFPNQ<|ZQEHB3yf*bq+8_oKC=Ab`I@)tT1Q~w zMc19dvnx^odiICn)ws*c*dKFqR~J3bn4cU?KjLS3^^$>X%I?DXx6&DeD3kgc?mUmo zh+Wl5>HSZ^bz(a-KYt{g*s$;m;SqMC-r7zV_Xv@j*_U{xxBqDoWCFr`d*h=^xgP zevPlBC}Wn}a_9L?TtYNL7!Dd+#;(rqgLFnpPlPrVh|=`}lKX$BNCQxeM5x+X&^}>B zVi0UTb={ONQ43V&EaMXMr%X$;4*Jyn0iF7&y~g;X{b1u-YGrCQhXxN4o?>|v%FLfL z>w1Y+gE{EwMc0W@ixZ6m)WyX#J}fpLy5G0|$=PfCew@)ayszu~s8}~?w195sxB+*( zemBs}sb4XarrhP^q6t$ozAA3mbif5C{~^x*h8nQWB)@Mw=f`s@6wdRjO?+#Cf@i*4o-z^Z_Q57ESN~}-y@)@yu z^iAVBu}u@5NzezgW}zA;bmH^QPXiPT3=uH%%octdr~5MGqll=ur-0j`m%KwAcsMZv z0+DL{32@$JAhsMH?M}+vnng>eH4lvl`?S~b6m3PxUx(NzMWgiit}cTfEo#D$J@`K9ubgtjo05tGciSd z{c0%T_?;R|Wr3NQY$$d~NgAZ?T+M{SOf#N)8i-dHH#Zef3|_E#RMlH{chxYP7@RsA{`Hy~*WVh~%Y%xAavk>RX4z$}Lv?2s>;inKE0jc| zuRgnKD*&8A*J6LpwnB~P%C+b3;(7GCK$N~jk?2k=B4Ne*%-dVC=kqmff|8q0Zd@u6 zemYrE@92>7@oU_($ea2^9-c!pIX)oYD6gAK%7{@ZetGpD+221{SnOjEPfPr>i7ej1ZE$>bYQ(MM z6A&N<1K>}&)t@j&z;kM$L$3}LlH^$}HWaP9wL{K}`sGnEXctrz!A>|VC8d+LbtgAB zFXvqwCpS!xp>Q1MUlR#X0_$_h7BiCpCu$3Q&k>m(Ld)#V%BVrGEscaW(cL_K<5FG`25CM=3=LKEaG1=v@g`vjW z@h<{Sj)&iR2sn+^KIBjhwFE(FeS5nz5C^vu%I*P+V0XZfvf4ypf4u1{{f*l-g%O~z zv$0XI`?0C=MXk?`B+-vLo0=QXkm7jOXREhd~V z?ErlBK_xC2JTs)=t5E_L6`@8BK^_|}wZbhns|!j!XOEO}f4~hy!Nw+{=0#gzIdFUV zDhPWV#Dl-C(OpW#)n*2mCdZjNR<8S8*6U~fD^b5cE+j0n)Ie_Vmg|Ai*PnNZi72~5 zr|X$-4d;9VMaa$djGGF^Q}0zMt1E4bBnNbERP&TaRb$mI#N#gLabqy@oJ-T=753!F z_T8QE)pIUCgK~0mLb#BKvlV7t>Ap2GA@&ZUbm?xrQVE7LEfav3`8D8KWMvu2S><@k z<$+6wEhzm!$*9Yus(znZ?(z8_bb?l;LY;{+x9}^)5VWq89V`tzR9#)2S~b{p5g zhgs1#ztU?xd#FsY>H9qO^RQIk4{*esPL-J6{{0&l_4?bLD;a}%8nFwh<;eEN?V)=u z^DLE_c;XzgxXb0a!A^I$aS>t9-0GMWkN?<294PFL-Q$k@Agduy*yv)adb9WgZ)4%U z7X9$`i!b?1Mz7a)JcPn5)=|{7?Sf?s3I^7CvYZry{J<@)hhk>o z#RI>?pAT&L1Wp3pt8sQemEa=VpI@H)Y6oJjcAU4@dzoDN(!3XZcV&*1^;W7sBwd4Z zi-*P;-Le6Hux}_kI`h;FXnlNy<34G$)jA zp4|A>R`Boe`$Oykk)$1#nt|;Bq`2so79}D~xtysMjjmZ^$phw@1&)!xsUBEkhTHR&=wBYF!EO6$P*+)1XNfGtKVXc7oU9Bp)*+65RNueZFt z^RvgBy*&kdR#OQ!oK-V`>wHYfccqV;!!Q-=FX%I63@H?ljqi_`=NIx<|ZsI-hdD?)KpKX5^jo;0qSrZMjH+ zLqf`&7r$(D@1t$G_RUs^$w=CH$5G*VkDUn}dm7RD15S?UAg;Hw1st}v-kGx$E+|OE zPBt_U#9gqynQYRrk{NO!Zl8XC6CVYz-YaKlO-5|xG(lLkaUOyBH0R1IT`>K%UUzN`>Fw3wQz_@2FMoUOHlA|et0DYSNxr~59!L}^M21-iwitc{4Qe*J7F%KbNX>~G}gd?(tm%6N6u><1tF^itS^CJ ze+6HSe&kcZqy)I(5cVGd5`~)d#bX~k#@`8lU--S;XMJ<`wn4~6LrmW{SiCHp*S)M^ zO)P$SMGh{fsrmU(;35$#!fYGWoNZTKaZ;1FE|cwe*F?n`7rh;Yn=6@IR_;*XBXA`N zmZCJM6cZB@YSq5p-etKjNCY?H4L{1ga^Bi!xxGnOwmQHlQ;-NYZAKVe#`)z+S{sTNHdnCr(f6uOA|7QU}uQ&{6l2ZvIWBw$0r< zp}*n8TqkM$_|I&N-NbI)LdH33&LnMWHQV`3n*hu`{n9++3hFZMrEw{#UcuxeZcMov zgj50&`pbDou!gicbBjg)2?ObW)@H9@#Mz$R?jh!Gyp(;vycn$bIw=lG_*K_#b6i-79{tesnD9*alytf z4LF2_$v}@)PvpPqvNXVjYKdkzSL(BU2U*R9XE?Ulv@5(;MU1!;ZOeH)UVluJeg_UnE;SAC z%NQGO8=dOdSTD08|$>v9?F>TW7#XKHEQOMv9FJUKsLog?BfYZvMQ3 zt=6@OQ1!_>d#&&Lp~Vg7WB-5pG=g-dK1rivUuc%EV*xh1Ots*E@Vn%2vRc3P8EnVxXNeWBNclVwTbHR#7_<&)Vu6MaojxEg)+M@>U*gzv3Szqr-m88!Fj5f-b? zuGz-K!t!{?B7g<#8G+Y|y5>KoxdJnR7<7lL9n+{eOiAY7+B&Y1vvGP5a&MCczIfQ~ z0#YZ+W|>58DCZ4kvQr_kK@tF0>e8FsgC6xd6(9ncb*ngUV zMXmy>K19nx*A_-orq)*U&f{lVETNpsD5TQy{UogPsBrZA$}a2q1rZ~3qGFn=x-P5u zz4M6-;9JocXusevEZVU1{jP?Nj;)9~pN!?>!dOUFX^nUn0uFH+eRo3}A16pqS6Z$) z6ke~xH}!OLC-z}=6T5t1_7880_tc^$)8fO{50f1f6?h^ziPiuvZS*Cwt3Y)A7 zixRl+^zIKH_u{^NUlQ8HNpFeK!5T7aQcge^Mk7J)La^1 z|7X|CO;7y9*FhDSlOpfG^eO*}0vA`S(nf@6%K#y!+z~umj9_Me7sK`rdMqNrKhpxf zm7h<0iQg4am?lHF0SfiEWFpqnud^&Dz@q$qZC`y-aM1T z_evsgfZ-C5b?7M`1p^GwrimMw4wS_3>sU~dEb0Y6jEkRc(=6r2@iY?Sv zE(_vzfNAYQS}y#L_FMb=^Bz7M zv?IsbrgE}Xs_!E#_E`3|7i513$!5&U+RJ;Vx02;choIXBDCTpV5KzMvgQ$$xv{{JJ zBlHjCn@zQ@M%a-rZ*yVp+4FdyLQAbXiSh98TKfCZz$3`GOesYi@NjW)$jHzmsxS!& z*3YjY0wa_LL>SOc8^5PuNaBLIaR#@JGl1v?dQSG>R@@}g)ADyG{APF4Rl|s%rJZcA zcb!#_W>aJpFe+4f_k?9*=~ZSnNLLz~n#y3fj9L!Z+q8qinhsRY4r7Z4A*x|yR$y8O z0Tv&hl##QQCSe(hr0Jz>%Mv2AX0gkNs;SR|GoQ97;^}cy*#*QGM4vVjKi4w6y}-|n z5vPt?`)|f7C?vapOh6=gFaiD?S5#17g=Uye_zbbBBi<`u0pA_&{l3KPy+Q05y?#~& zBQ8C+v`rfM;bdUz_{~)}>{{C6*YXt}J`W|eg?sS)pQ&L(5uXbbCNi!wczHUh*%k6~ za?H>QG3eV1W<-#diz|wtk~C@bkp$xIhg)6$jL;cjDP>#Vj%A0}T>fkMBs?g^MwV#7 zmyFrG*6YvWHR`ra>@#^9n}zg&L9;tmdem*CQzT+Jcx)OjPag$2)yLOABAmbHKmIHl z(biHP4H8VLm47^({@7}%Y7K7%P?)UHe6?FAJo%{j6gG#<((gmlF zV*5`%_I~{0FE?D9C$RjY`1ASRRFf&6gb#9rMxGDs^atcE1we=*K4u?;}ualEeW&L?W#t z>`?nVLp3*S`qIxsQ_RiSm3q&Wpo)rn-tFJCG&Fe2g)W}Hb$mVaTVo8kk?Pt?oQsze z>Tdm>+y&e6!)wJ)C0O~E>Iobkf2@AUi|MYHGIO4MDogo@3MCV9!j+9F1?b~=iiG-7 z(8_7wN-gIQXdp^BFiTBM4JAwsFH8p)85l~#bhrfu(x6YmD3{JS=jO*}KD#wFHDREw zP1qo$(tjuVm;vtl#PCIcmlZq$ko2zR24l|hkjDp%o?$ucLjS_$G6IghS9U;&-ddZqSQC5k;>91>D(=;YmlQ0ioyP+nE-rzO9;wEz zxYacnG`~K>CK8DWEtI*o=pA(|GN4`b;fudW^602TIj-R`YdL=P1U_U0Vq@d9JCC4& z49oI7WO23sR^kNld#Cp)bphPAp8NLByb(fGOz5lW|5uqg*q1IaFj#fEuaqr{%_BECcW=T^P8q-!vHc_Q6ixznff1(w z&(_x3e1NOf*lF->;o8g0Znb^6{8gX*=8yQRW9c;F-1UfZjo%SGggqWV6zOYl!Kd)3 zdG%^;>M%gyv@8E#TT{CYIJdI3Izn9OP-u!5p#x$8{H$oDtzUYyR>ObP&xD^J3Atxo za2C8`EYReM9zm}AFRz$Ib(Xu~%!;eG+Rp8#9!xQlUib`oWKR%B&hB{{ks z&f-wp?wIK!H=q;5Mas?IjR#h{wPu2l%-)k%6Ei+i@rZXHol5EJC7^WZ{4#uNYfGGM z_VX7UkLJQC?KKOPzK=Y=S7pITHg)l#%i_}I;-Lz6NdK_yHT3kt%m7d_$5;H@0|g#S z?@4O4yP1=r;rx%F-(1dTix4&S>(})?x_X?c?GMc)=vX1&twr=3qayyledlY7!{g43=Bj;jfR0#PC&-Y>v{r3EU1d@km?^k zd>9@RBdwp?btOXzplUEXp;{BPcZyJ{JIEaDu3c>QKe}9CBt#e#2C0q4%|*5DyRsegH?_qUcbbamB1TKNpx zqFKG?rL|+=a@4`bgm9XiJUlkvD{sTHXo0pm9p<`e@P|miDZ_xDUok$~-@Zg$W@Tq< zo23>-YtrQBow`%g<(t*um<6USB`DM<9_-#gtoEQXb@%kBf|E*ITs%^~>)9&eY6-0Z zyN|6pR=l>_aWpbV{qexa;A{P}i1Ef}H&f2Ld>H=s-0@^L;Do2zd2-^G7~^&CQN&|~ zL2ZDJ!X4gMi3-17?ZD^CcDI3mm|vU8zP}uHcXqlE z48VF1@pm17f;tW3_pkFSC@7dz-JyXQ3x;K#1EL+s4+J}Flg^P(x$NOeYJZ;xgE@N^ zO-maJb3j|4Sv5gm*XVy2Y)l&0b_@tCi|~{`dkaQd$2z`TDRHjn;?rZ2a2Tclch^@1 zGE&k`=-3boIQDa29UFP=`s@r0)Q9$GYY)Y2x~71SMPhya!L`{3mj{2<%=-q0=XSE1 z;})1V%G!#X#%ADKMZCV7M)_>b}GZY=@Q z`hgTJ&>yJ^1{O>C1RVpEl$7q(JRsZwtn`T?A|(|n zKY?aT3QEdTb!JYLCXZiY9JBP?+{FF-{76_bOv|f%TP_Rku~Mj^ZMvdcq0B71zh(~D zIKd}cavbq3K7eo8G3PKNF6RZRt=jSRg_EN|PNcnJV9){FWR$S5@C-mrtUh%o-<7;@ zC`#UXoA1%R|QQ z12sCZ7AQAXz z@5Z@#GtZOlZWB4b+YvawC}Q_4+wO8^`fWxmE-dhX=^P}sLD?%C4SVxx){q|Ecl300 zsG-@K3%={0oV0+3nwJr0r5ZLXTS33>e3jv&k^NdIgpkvO0c z&XLXgy-WV`gj8v@uhL`{F2zuxY0iIlmf`6iI%Dx5!lPqptV#dBb9DXNH1^T_CrU8( zKhd68nIe}g3~VY;Q-PQ61(@7QD{R=v&`a-vYfd>$G!R)>SRf|p>=UzQe-@V-?`H zN>{po88n&>9vT^800=6zr%h(bG`zo!>wp{RXxaVC-kSlP7)(@h8k%QJwGwyA;?26G zy&=dW9;7OYe}oLI)}6PW*TH-Ot;mm_vmh2FO!kb6M#1UJe=SD2@yn-_fL-tr)KP&N z4XdBP3l0_x=64sX`8l4*J^6Z2O5tTV@Fs#+Wmb#3lhxo!`poIWqjD&>KK}Z0k_-i! zAwuhAkU@R|Jaqvkxd*x?qWxL98*U0~=MU^GBTWby-NL06RHdL7=|;4JTn6TtR@2rK z1K?ZW4KPrvw1N7XOBT}2Usqp`0CM$}N*hnxd151e>cXKa_$gRp?w@l*i{HH;`v5FM zq7v_m!9AS*{P~_HW9&mP4*|0a?>zv&mY-j=r!ZVyhgz)xap%;o`xgF{1or5s{*hC< zq-`)5T>X5*@3uo^9wyGlAAhS9}~bk_5Q9a?f{;03v}dF#aKiu zwlOe4S8H@;Iw{8Z%wIjw(b17**OR0!Phg5XC9igVTSUPw*k}*swvx`>qF$Y| zBY-WGC@pq65Vkv_`T#BRGmcBU6m;cwyxU5^zqLVk^9PUU=4M4&5p#$1!0tkq&(Bi- zB_(LBtcv`RASeOE#iq|S)B2DA5MfB$MVwvTRSMhrZG{`qPG_*24|aDV3|}+<)i+4< zD!i@d=7%i#2H&0K*2>#oM4?3wnuN_3qCON`k{4B|80B?TtxriNNm8IwW03xeoa2id z7i$z00DdYSBUu;aM$J4BmN!nF@f$dDG1zRCKvrUDlFZ&i1V!Ej3h?J$At zgv*@eZur_>_I>FQkintQApjp#D|n$jnqE5)>!}$Th4e52jFvmB-?K>fo+fcSe&0Wm z0k9X^Lu>|e?*p~r;_BLpP*O39$Mi@ijE2VN`Zb`y@KM8!zS7VLQwF}_#sh#g?9h?} z)JQwbm8HFX*g|Fi>z%5%W@V79$q}X&8u!IOMTQP|Wu`QEVUMUK3aF_tKyvVh^as+* zdAUTC)Ir~}S%Vjn6x*S73|R$^=Mkn<9~i2Rt}YC?8=zNuE;3m#s#S{-T>WKm2=YRY zP#?`S!=(J~$SdnHU%<7N{=@DU3kbcl|7v1cWV+6AiX!mLtVov6$xm)R=UWpE=d*Ki z{^`?%Hrq0I8Oav-o4cDpRz{(`e0-8jev01u%>Mix7eoRGprWzcY|d%7j;s_ zPot1FbY@w1Ldbjw-CF9q>y$3*K@7Ms6kb$;vIN+7Yao{hT3SN^xhy`o1dd`siza6A zRFcPxl5OWc$vu+y-LV9||FuyOqpdiD*^L`Lh98uZK3e|eW)L&?3-c$J4-IYYAe>0Q z;ysIr8-9dK9=Wyrlj33euk&6mQx7#~ZcjAS_1S>4dxhVa#$#hfjf6oQ53sSzy?bAh zQ~<0NnUzp6GG6=_378K3v!41&`v zV%B&Fov7N#{NhJ`_m=B_5kdzyDbbp|(vm}22UnBKgonHZ303N}$%&z1gl%=AF$&f- z3DSA%f3$6k+GDXwe7UrRR?~MTxa>MA8SUC=n@VCdfcVuQ(_4X12{Yjxo8|zJAXJuy zM)k@&U+J^P_GA`dP7H0kX!ZF>NApE@jOI)Jrr95`Xpq-ZAT5nZLj}rZeX5oC`5J&h zHCE|6UkR1`omXMkxPVFDQN&qGi zmKfCM3VS^X=Lu#+$$*F!-OwP1w20k!>#htNETjZr3mcR!@1W711bTQ4VPUnEsLg-0 z!=I$^zb$L}K!4)F6?L74^Ji5Fqw%*@QR`#(DS5@@X3cHc*Z zD5X+{W`)cVWtOQz8B1m|r6?)MtWrXTP(&yskz}q6nKG0ynPrYdLdp-tUClJ(6H);G2<)9SrV^>F*#{-*83+FP=9H*}gs2>MZR z>%=L=&cTt;+A=XcEvQ{_8zqk~9j>y=6QyfqndaP%9`&!yFZr$G#hah3kB2@1e(EmU zW<41%OF3AjaHW)7r(QKaGht345pSGoQ9fTkCYILLjz42r#L|!mLgBa^0txc`poA2d z(8a%CqivSrR7k2`%@C>I#rZ*!VarC`&58lnL^TRLFK3#Z38>V^-iwBdNauxdK#=|s}&BMpbTo98&d4#u+@Vx_+l*Q@OyY}o^x!rA?3dg3-w>L}B zsq>GM&Mqzp?D3 znoXr9Rj6mx;^xo&InEcJ>MD5^o80!PG_dEc{M&h zzSsl?vB^tTX*XJc1)w#T_o88N$ayGvWeprAiaI)BJJqyPh-a$frCmj_7cFj)=!;uB zVD|zbfaXhcZ1T5@le@dSJ@{Yxf?_y;|4~S`f9-%Xroi7Y>*0eHlo|6w(<)7!R)d4F zN`?!j#UxFrbZE*-)WG}A{xpvN^w8j)(=2eg42#F6Y9`s!G`wg2v2q0_sD=;Qn!Irn z*Uz#(_4MPAsN*-?dC<76xZq!h8to{w{`2g`8=fg=?0J~snA!xwt9XB@s$9xPD9+A> zmC#?9488VSoPO2TVMCi5UGQtAbDGyB4&ypqo(}J@+W+v{{Y$7=Ec}QZKXp=*t4VA{ zef>PuN#OhC=yHjdRU!Hi;&w=a0Q5oEVWkwDU6g}RJYLRup_fU69+{f*_{oz#D5cuJ zT)cEi=uE?I5?}%06S;~3js!x#5Xtjxy*RIb=-t0^pdgiFS0H^;wD9G*M=l3?e2$yZ z(OaQ6JJI(B^~;Ouq@O%hPI)C8)YVbyLGQTViEprSNmNH0xOh^)9s(f&dE)rlpQsg~ zUKPeqTxvu#hx(Usg`C}bo|Vgv^PI3$Q`-0_slVVs@NEbQ640vo@O`R}RhZ~0_G0JF zYLoEv^dwYMe??HZ1QXApg!Nrj;es~dp9|b(Gvt=Bmx3g%rB{Pv$U;?x*T@x2=P!W* zgGdh5u)AMBHf3alt@sU7G72~D$pwTeK?}YaThgxi*&F>(6^?ul50CaS+$esyouFx( z=9v8PS06V^#zdDdm0fXipP&L0fR~Ge8SrL9L#x@NSOi3Tn6UH zR0D&9^069696gSG*&ZP%sZgMS&gcTJCiaf)GH$wk83j3NOHIvNi!X?2@{wDV;9`#p zDf^eNhfwXx{z5b2Hq9wXf0FgHc>vDH0ATRW%adG~E?kU-F5rTL!xEu})TzFECynKR zli;-QN6*#UVOQAX+!?EI%M2-6?G*sMkr8$X7f)1FRDGP%Dw3EYQGpx+8nOY1a%Fhf z5AU-VwWbe_9~_fdpR!QJ;V8iB{~J4l@}l(JJGz*d7^k_>mrHp?_Qbjm^opD5+xuOc|2ag@Kq-(CXv5INJZ0=i{{Z&l#JaEO`4 z|G)>x4x*MT!fy&Bbuzy5Q-qy4+CKyHYdy)nlyUTEt2Mqrcf4MvRr|>IwoERdfj`UD z2F%lLoMcie*d7bI_N8Ixd=95-k~~^s%8ts4=M9r%!~MI%cUE$FQpeqGgaTAg{g|Q! zkFcEg)3;CAmF*0*wm@XFW5Z)^Nmu;!05PsXEf5Equd=rG5U?hoi{2j-)%!gzHUr&E)^TG#W zM~nbeok-Lv4Gsyp4o}CKX>oWfAqCH-v$XGW9g`*wO!OOGD9dA9zl$U7k5c5jfor@$ zRj?PT+j5d2bpJjJ)YkEhA$OoM?MnzZE?bsMx7w9Q;Gc4%P z(M-JOePKE?-b<=XTvkFLk{z{Jk0jh0S&L(ilI0o8WEgxjL8}*tAj)$EOLg-N{-TyF z%&|MzLu!$ZN}+%wYOjd2IFMtLGUcqjS=J+e>dLF~$I78u-9cL=H$lm=iWGxcSm2ct zF#GBvMPurF=TZ>NL9Ad{NPWQFGod0ZhKcbyQd-R5K5Y;21)FSbLKY~lcM|d7*nwqy z=ia@WM6lO!D#va1;!h;X;&t3a=mn*ytc;p4lWN=w*qG^fgJ@r94lal!kYdx+)YSLf zI%ra0uRK#{f(FaBErVLH;OF{HFt!kjE;PmMGXMoi>9>BvS8O)23jtd|t_u|@tH<2v z0rDbRbwYwf}s?$XW2h3(}@*FwhQ)_-tPZ7dE$m4cNOAx^b*5BFwuib@(wF@Y|-eR}LY9;Lm{W=yaD7N+4 z?c1x+qXmPFwO!4xpLGy+mwITHetz%V42ayR_a<{sZ?ERr`Qy1NT=_WKsP32)3gZe2 z6V#=6NXRQ%xa`ZVuIJ37kC1P$U_Kb!-9iEf;fW>bDIzw_tAV|;$a+ZO=K8hmh0Zf7 z3QCv5sFg3rd~|C!aJpcq$8?gNZm3o!R1QH?i3aVHjzC!S6+%l#(XVl5m>p5WNBe%vx|$f z!aV|VBdK2eNVr`1r{4>6V-tmof7YTb;Qhhe3-E!#nTLm~rYIYy;%Jd*>Z?PF7Hl_d z97Thuj&yx|ruo8Hnh0l59(muoMN8H{vSMP9kmWLc1tkpjE=gyCdKtFyozl`<@f(S{ zZ5-vR8O|888HnQVXtcyeR61pdNBEF)&VY~$3kwUauP<@o7Ntdtc>KBkM$-2Ic$mPS zW#{CiSzMSCBwPS$DI84P_E!e)0-YodnoKjtyQ#FUZte9QrZ_y2xhRO;a$&HVm0&+uoZ~-!ln^in!qu0p zQ;9!>bk~ld?-PR13w6p)JVm4i+)Pa5LWOq&p{p#zBFE(-ui2n1(*I5Kv<3-GaESb> z?b>mVb)VHE0PjPcc?SpphPOC;RPKqoq2ZRexVXKiYqyA;|HMMXL^#GC|5@1uunUri zG88o*V86zi>4$#wqsJ%Ni=U5>hXio|5zC+^Spk(i2@%4Tpnz{VUb0H?5pe7!#w;}+ zx+I*r%dt(LUP+6V+3x;x9eVp)xQt23hbFCYswC%pC)?s&_abRui7^H?2A_Zc{>y{6 zLK;(zHY0@R?Okr|p3%_&@H#~`H4KC%BrG1V9O9nSO*5ssv}d`HE^{|bjA>_^c=CU| z=hc0Aek-SX4ADcAdq4VN_>YK;jH}CAzyqsuB>!Ba@li_0w>+BXB0iwRy9mh5x-xRk zHYMN7?bCw`F8O#;oIBsjWhh_4Oly7fl_xA*(N+@ghM7s`WA0*f zA1Ep&JVEJ|eb?62)n!4LrO~%ZvcuCi&Ah+It7Nw48&-fs(CO;?vC*?~{m8E5mex&X z^y0`T4j?tzK}2xk;ZadYMrf8@_nT_Zh_;Y>AqG=+*LL618G|lQ&vzy&urJd>_ZdtI_CoQo`kgqXEuTL!%0TRZDHb zG^s97M+m>I3cY4+b@tWYDxSml6zs~3N~UE3hsBadgL#jaR(BmGaW9oltF=mP?CgS& z)wVI>U_^Bg0r&5x>gYaH-uk3ObL$F!ApS;@CjDy zHW09AHz%jgSz`%dY3_^JL2Ih1>$T=)AOnY+$rsi)w*x#(we%6nBN5l;Z|ql1T3owd zv)UwDZC_E^-eaGr6&ynzECA>AIUr5|Tt*{@a|&e!}~RFz+x5Tgsrn2y9VYi8xE`cPee&~lgGL7h|)&lT1*FE)O^ zC$hihtilF`?uu|a$;I2$KBGcgr!tW69ewae#CZui3wvL4WpQ(*(Dnvn1CxvbY4((( z1!a*VPX{A?mxspOGOT~j;j8NBrE>h9n4C03CV{B7d(d&0AnCUE^XJ`{#me;a9Qm-X-#EXG6-6LDo(t%$lALk)87^Dvs zI3(;B=ABp2(3lYm82SrX2P~gz>*ez$(1&lAR!#x=bu{G>%kDQOWsgVI6`RL!d_i}xW3V>*HoWY`xZZI|mthMNA+`0i0qR)X zzBJ6t%*1*Qy_nPdjBTF#oE*|LPQjmupaxknRgG!DX7n zIfUUtt`CW{910qumEkkUNqKvpXy)<5OA#fG5`-Xous^|On?}T@P=TkMB z)@Z{bV#Ue*36)M9?8UD<{^ZtrV1K>UR+pwmzBSUE&A^37)UCSyi2uQ>iv#qa9Kq^* zcy4e&Oj44x&SDooucQ#;I69qD9Jf1HE(1t>^yq4eDPj!KAj@pox)rJ10nEluv2SNo z^HPDU#-T!cs+B+=%nQxz$h>#vcN(r2 zWTu}VVEYs<9uMmcr6KGdqvotn+)5!XgOE$B)&yD(?@q#p!Bu}9m{%NjF8s>%c@F_3 zg$WqZV`m$}Mn|l(cy8&?E04ftGK91*Tf=rVaKP=PJ+VSRN+7b<%da&+c5Mp)B(N8!Hz*E;-LhUeAPlYJ`L;qk$e-=pw6zZNd zk3TNt*HGG(l7ue*f~=897)%iwuvSu1l9&w$WPqN`hEhZ*VAJAdZoj`KS~n$cBuRf# zumKV$^)Mo7hA)#20*9g^8e2l0LMYphjaUZj*n6Tj~pA)R=E9d%YpKr&ufd-CSUfv&0-d_WDTZfyp$0<6(_Q*q2Nsi zb?oY~h^eV5U0fH$Bs??Yl!c;`wCmXZ{#e&G(@S0d94O$oWo5n5SGqnZJKa-B$f0GT z7(4Y+ryz3wrCm2^AxLyGUfd~Epb~U;fHZ`lnxP!Tdnd7+IN^@imE@t6ZEb7o#o5~O zW^F29PV7IZ-4vk`BMcH?4Kq-Yl&kn8-#+QNBnD_Dy#-AEA=OBdLNfO2p%b5nTfw>` zho8JL0-NEipj?G`n263Mztjl|^+v7aD^jPqF_+EI>3 zHOCre5w`G|l1ajr#0^eaq9yLCYh~YPgk;jaFx9)EXAH7jE7i) zI(q`hAi=oE6T`y2d}w~GLRCXd(+8K}u&`6Q7-U<_!jeu-0q0%P5{bn_O$t-qC^j z_i>}+o_u7E1Yn3nYQO;6Fr=}@*9rkv;M=bgaNSc3=^2!R!Ta|hO4xXOa#8`{4=L42 zL`r3KHJ|80AJc*-rZs$sk(HL-iiAGCkC*Q`Y~@mmA}4(IBG-xJ<8s&mu}F5B{V_ma zAwZ4OYzhz%!Ax*RA#jCQf^ah&obvQGL=dkSiL&VEr zIS|B@Bw6B!5`w)tzTEozTd?#3t#DVpaob5FxivJbajxKCfJU z@%)||y^Twsn8yAo*e99|?7rS*e?O4xbrS$a-gCam3FhgACpyp8ZP}9YN9@fbAOm*? zw5r^@u6NMO3i>4!GBevQuX)3feGPOR(lvbn%YK9EoHTy`dvThQK^|dMd#2_iS?F6t z!Uu9ya@SzQ)8bs=BJx}mgC|>XW`7b=efOx-2A%66NHVv{diVpa+#=;vKeS;TF2r?c zU@Opx2S-Ku14@pC2ZWHlUH=Nyjl+Y4NVFU%8=AapAQ4za)fhB6*#DU3)|y{}Gdo!8 z_9DZvy+Nu6BG7v&dAyUX8qM;e9h1H&R8?$4MBN84LqfY-)*`~<`s%-8^N($*vnv!i zj~q}k*Mpr|@-Ub38=g5@dku{{Inz^6VL~rj3Us;tjk~Lebv*^kL_~1|6(w++wEky% zEWe-M0TbE~$OaIAJl4I#8;FJdP^OerR2=>yN+`$r5%hoAL3PlrV%meX`I%3z zgaAiAy;R*>dJAroeqj?E;5zbS^qDvShk#4Vz@kkw4~S4J2L8+I7So4}FQPlU_a>FB zJHY2k3+fP7(B70kHIx?cerDtyTU*J^Rpc$7dh4@}LfpaTAmB*AISwCfi$$Dcjn563 zaf{I0@r>Ut>#+qVR7*28YgOTc#WLu~?|4XV_NnfkGUzwA&P|g!yEzgk+AtfLK62Ny8P6VZvIZlgL4qw+}(#vWP4X~Ck$2kiHY|6N^ z$`2sxJCSdoF8zy1KgZS=Qr4d7ZqJsE+s0A&=XUfO6_6U;MyXx`Xc>ngf+*#aGSA8Q@fUw)$noB zbC&nsd*Ql+_E)Nm=70S#nO(J&NlfYd*)_yd5U^k5`j^QVOBa5H--FK?v!d=Twj^jJ z-Ds%nOjWTo4z9Q!P}8?_(<@^P%yD*9-*yWoe~f^caa7!_9Ad2LmeJ73`*3?{n?cZf zH(RhJxq?UDHeYF1U{o=Atu~oy{w(tq`k-6d6>&AQO#cMBon^UN>}XVP6!GxFf42!e zY#@p(TppzWZZ81w2K_+FZ-6JHKBExL8$p;!)sDM44tV_ieW%xSIrTGmz1?fyr$}T0 z0HTn$%1a6{?3NP#!HstQzI*hoGA3z<~p!BMDEQnBl}s%giLa$OH_C z{n(Qn<-H)p$jdD?P-D^tAXX^%6ZVOH&{}E6A#=;{<$0rsX?HM`ye^Suo&U`ELFob3 zgm!d_3}V1-{85coO9L16ULc;MYo+dVmvuv3g%()bAk9bv^}khZl%B;LMsqAbefmqo z@)K04c6suzcqxn%ttWib<0m&1o>2W3tFbID(WgRnI~C`Gh~Q!Bqbjw9-m}kzdrd!2 zGD~&8xl1kfr(9B_ihsmB_vYMP}}b25FDYfhNndf`Dm=JlR2HgCu6c-C4uqx)<{ zJuY_rxeB6`BIz<--R=2rK709*k{pV@9=xUYx0QDQCE$*>gbrJ`(Z?Za7MTV}(UizM zc%)c^HzwFVqVG$JmUMpk5i{Pn{IbEn&NHy(2vlf<15W)G6lIX(R{f-zLnO3Xw=@H# z@^W<(XC6hPu*Fd9j1Jpea$~sgD?Yh3D~_$8tZ<(bmpUGC=#Qn|xEqD|x%N~$u*#dS z%goG@c8MGr>p1xz@&;Y|4-AsKBqAS69n{XChf}e<#6=7kkczw>7t| zF|7>u?D;mzwQG2@YhKTT1NuyJm%i@7L3}3#EqUY$0RF>}Sh9$*>>K%F(}-D;e2h|{kD98g{N!*5p$>Itr9(_yhB?13KSFv zPNyy9$N;{RLyM3OSq(uYq5mb_oy_9=B|>+5OWX^S|CeQ890!EWOV|diZ~s`e3g!j^ z#b=%aFuanEPN;ozV#$|xho_d}v1je_gehMpKGzhq)z!fM)VW89=`fSh3Qzm>T&Z7L z4n|*`xH-}yiD`3m_@jysGIcC@8lXASkp)2e-@dMCi9d9bR zzq{ZZYIezmJr!@?l2-SfyYb(yRDCKDzLqh*-i0fL;uweHWk`swvMu*2a(i+{e_0;K}Ha58Q;kd&kq*3{u<>!{>qFS}R`Wd?VadVwd>$Oyw-@n(}i9{w^ zfY57q$i8zS0Vq?t2J|&jSsy!&@%%8fHQSQ2?W~V$kfEQ{$*`C=!ahh-vsAYvK6x@+{cbZRAE7rfExAEQ)ICdBkWp^qQ|%BsVX9wY zjinLHwUpWZW_qc=>cQR0j#S6$qP`wFvK6+$PfW`^FbFR+`eVqD>Lypdqa_g1Z&Y(> z!n8sGN$a1ACl&T}>H|SE=(_W-U`booVM%=9sn^ad-t#ZLpz^UAjR=T~vpRuwthb9jCST>{?Sdc}p~Vs%p`oFgd5-6x zYI*?>!=|N3Af;!$-%?W!`Y(V@`xN5v4~C9C=qY)%s&e+rJEqfc`DxhMOGCcNADG^< zwsJ9{LW$llUQjRJjVu0>vRyGoA-sq`m7@4^{4#PVM}F!SLZjbNWpn#d{WA5zSfe{4 zw03b@y)mIjb~h8bn=Jrc+i3<@@gYE$;c( zrriaRiYx1n?S8K%nWX&KKVoRpU`oAF&*$@p1d_$8f@S!ZovV#be=TlJWx5CT3uZss z-FG}Iz>qW?{fncNa+RSCie&T}YYm$lb&0u3yU3Ah< z+Z|yNw4?P!r^Lp7T5v{4^>Wsht={Jpbl&ow4!fM91^4PHHO}y=34o#mctkAu4gKdN z82o*99IbNV1ugkOh0veF&x-khB8_^MYKAC9~@A{1HEX3{6)ec$C-io+8YUWctJ zy)@pAg*W%J@{hYV?NNKDwEr+16A)QI68v=BhD4Z=$O?2x44cAbynV9s0XmR~W|DC2 zQnBmAq27%{1nna^g5b1iByS1zzgUTPCzwCJ45jy8U`5fRDECVt%J+uwu2v(FeZyXh z>pTcd@Rv1v#hj&iQ`Wfoq{$fpx>T9#JBmIk#qvjsk9~fw9Y|sET%-TUP1}yuW=*?c zFZ-ci9zIE16U}hJMBNJWXNSRLpH+1cVIH7CAV4q?JfI>&xOl>vx;Ox?s7KeT^x4+98wgErf|A z@sO-K0lE`z4Ak(xKRywDGdk9I2wg8GN-5=34O~pUe14}0uwQH1R91bDAEq(VcP!j+ z#_G}bWd$wPEs9!vPG-zLyed)0L2&|d9pu_a1;gfo#EX8l`qJ3J$ z-z2|gkLZXt1J}Bdc;y>jaVm>v*B5BpWjuTBeYbUf=A|*M*GCdFnG1 zbHR>Wg~`Yaqb`YnZ+?Eh9?c$67eLJBk0rH=IS$G&z;V}56{7D8s0e!4SoXpwZxfI~ zq{(asMZOYLdOisBN(4*+1MC?aL&LY$9||O%rqFIyumA|xD!sE)K?5)s8Gis$idSe5 z$dM`90x$$up#>mATr$lbs3gc^co}eM%#-oOd8)uS(0}CEJVX*osNKo5Scr&v0R&6T zJ%>n)G?2iYiJS(F?1+@$%Ii6{ec-H7D*)3w1gRj=@j<22QyDz3E)2ULks(N72EbBK z9ehI+3UU78ZzMKfu*XDNqvBsE2Bb_0re3IQs6cuUeI1f7>tjr?u^_I;3WYFu7r?Pw z=!G&{TWQ8HbapGCgbHGDPVxQ&)=xCD);^A8K64S@E>x+QfhWM8}v z(oam&zQ@8xYKAu4$Iq{!?F~>^%x`5K7O_x#!t$!z@MQS?Xol?Iu&n)uab;iU<66r* zZG3(8wV950o1Y&Q!IGUydMQTBfA6vBSHGltzdRa#ww~+E-H0~&mY0`T*B>IXclfFW z4J;8g!|5+VBnF5vww``hUHul~jCA}=;`k;c8OSig>EMR$!LP7j!IzYjkU2G@I5KKV z3Frj24HSnNE^zC{r|U@i!|p&JSb*ypK#7z9us8)W8Y`z4xeVnffgWk>S%TToFp`Sr z`t|GNU&#@LtRBI1eqqdNB*R$28bM-1s0)H{Lu5-uA*x}}FQ+iwiQqs)IS8sL7PuIe ztU>wkP3aaoR7`~(hNkExSq+J zHgG2XV1Ul5ZUFt{4K~UY)DrPlkxvdZlnv!kqMn+rY1+~_Hu6B>#(G*Xi`Y3YoTp*G z@-{v0nX|S^^(#yX__lDN|M~doRPW_ZZEAtv1GqLhtNA5;^y6jd-I8@Y3S_ICW<#}g z==zSlqX|dCzrpv=A=b2?z# zN&$C5BD2~bJ_KGvGKwJkC6qCgYs7Lj`u-^pkgA|AX$i5UDp+v*$t5Qz zCsP2|civf$1smf$xFl90IXb`u2>{Ut{h}O%Sb3&+>oYjRc^!<(Ez&}~G1qSm z&Y&`UEaF5!vfVo3E`i+rT!UtSg%|?+`eCC?@UT!E*d-=b@P<`J)*4gdc zJfi|%-9Fn5J72%8t?#R7Z*tV}o#ARWk(Gc@-$Fa@17KzsC`n*z&8$L&SYBv$R{^o} z25>{r90Crqva0J?IXF5tqZ5a!2vdVSqqiwxvf^5|lrC;d9 z43*^+rS?aUaOx6H7*;PQa0W{?)xLe1c>WNd6rm%k2bKb;y&q3qX^Oj52mOx%c7bZa zlj{Kgmqi?X;{L6_Kfd4%6z!Oe0QmUe+~Nt`2f%fRQhH-9r}@4ipYYQ$&W95B`|t&Y z8eVzR2=&ZKmv!`mRk?_i>-fgeI@`aNXu-and-_<;{QI?_G}pzv%207R-_?9lzpw7w zaHs4-NY$P79R?Yf!f!pX%Elp%Bb7|uW`zC}jT{*)LKJIcSR-sj$Dipi0Dpc9DMNzx zGrgHz7vva!p4WbMyEfcN*WQycx#=tsk)g2@_+f|#_5NbgfQ()7xtkA}mC_q!jaTJd zv+5363U?s0KF!S7iS7|c4lb4+B9FgvMX<`!Isnm0M5P9jKOM7Dh|n8q%srQ%-25>w zXWX!ujzG|(oTZ?9974<}hgj#WR>DyU4KEV%p8u&l*bmL#O~mtjG&!|f+I=!XhbD3N zM|H*(PY38%2CAiHrr$U~BHVA8uj`a%d){EMvyDIEqkXVa`dd~l5uCMIUN4|_3WUap z_^2EmXSdHRjCn5ZYkmFSuzOo@4`}u;T>2<`r?H}f7F;pO6sDp;X1(Ob0@P+kG}nY` zZO=KkiUQH=7E}?B0ur#tiGqME*HIy&Tf}`<d<61xGY4FOx2{IZl|hH`>J z!9#e9J6i}taY*rr+M*o%po5D9FM)%}$^$w}2R8Vx4{$_~+8$C9EQ|?sE9U42oDfWd znY95>lUuvG;r#P~wS@FN&;hf^xQUU(D4c$2Z^ht_U;0BZdyyBx2w@Fq7x}q)l4y;Q ztkp0QL$m?sR}lm+^*Ds-OD9Xy3WxyswO%38j`IO4}P(imy{crz1gn3#f2Ni3l9!)#Z^*U?fbRe!Sxg z+1&~6J${wdmT^b%hZ=-+8SKD$*bg^y6M`DnHCZy_9YtR+TmjsZ$=fW9>BBn0UIxOU zlF#V5f@|SHmp?`c9xmp%mLLLrA20s+@ZO`zFg`@dnymGzeb(34M-1_V?87snT*a$l zJ-AT9WbyhRJ)-3{hp>d>Uw7y2^w<^^i#&0k-KWxSOrrpHLd`RQl_Jl0gFSTr$b+E3 z>IY8(i8<7u!XWrHSI}SoSZ)15eE5*Px(s8WFUfFk(#(QuNmPGGNM3Z&ckbV}#uujB znA-n*h-)`Sf`lSf%X;4Dex=jViAhWRJRpKZSA?qu?snRUqu%(?JVxZ&kMM+wMiyMdd zzQ!|^Z+WzucU%-d#h$SqQhg%sP*q)ir_hH=Hl1B~wI&mx60)*Ha5^h+?(5Rs4g`RJ zcPQW)Sy+5c+1BYVNKw_kZ1w)QWFZl!(8xTsbjW zG_m-ERnp^^M|uLo%_`Oz6KLGzIYT2pGw)l+8dFU3{HrzwYjfu59Mo>UFz*%9ey)r` z#aw(jo^!-#>L63xeMH<@QH-EHa!@Jf&HTs#7)^j%a<=FF|hDfWi*l* z*TqAV1$vl42X=u4Q*AXQ#YVq*E-ss0-gKWNiv^2gXvxWkICkQe+`r+Ns`J-d9FL3T zzO?Ip1or!N%EAl+lTdrb*RQj5FV8FF6O3$Zez0-en{y-i8u(d6;`vR6a7nJ70~ehWJd{sN{g=O5 z&N8(-tRukZeeJjPUAUv;(R*9APlI3}_|0`MFJiuGZM)|gmQB2jfHeR@a8C+Fs@Wgo zw{ym17L1o?mzM6d>P|UVz+n65H&qVR(hN15u3I(VN>$i}_gS56(9nq#_C`lY>V`(W z?);wBOF0s_lL_5JY7L@xfr6C`StK1VBFVsaut7Qt%~p$W^c=bS5dBd60u2JsAU+2~ z2Z2k2K!x+Rb!7lJ!BnGHS649$Fgb_v-Y+FyYRH;aP%43*;rSE31YXtU=(8W8^TG*X z9uaCpA#F8==d8gGp+8obkVN$c6409Fs5X~>hXDDEFe|NtL_wbTgtu&6rw1o^Z?**n zn37?n$JS>2=ZK-D*`F0=;hPZQfx!!_{yj5Kr3iE2KYaL5Cg5;t)qk2dhq0kt>>p44 zE0SsI|2|yrP{#i(D^@h-N0uCVhZS;reqj51M(i~!ZG*THPwf`=p4{K`n?`et0sI?Z zMVv3hVV9bkO3XH>TrL2?(+bKABjY*$84bvpe<A}@{{7DVe>2pyzdoDT!-4! z=jxr>;7B!Umeje2N1sLa1AB&k){sZ&1y=s+L+sS-Os77uGvBi~@J=OQ&kdE7mBN1S z{En{nd;f^7&TdkDVPOkzMLKKU*q`fq)w43)voc@C`L?b3A_)-aBWnC?2x!*yV>vhC ze33~VR|hw8^^^i)-EyU9F69)hHQMMcH4q@;Ia(#MV+^XT`S zpNWNo=AXYCa15Mk_fgp=;;*mjpRfk9f=%4qeMI~-e#*gC!{y~Hqn}=DqhgH|vo{O3 z&uK1xSld0wMq$+dBOiVB`Ia?@Z;h4TS2HiXJ$;}n+~G8X)V3h0ZR&H=@W0`>IVs^Z z{>Z8>rt_Jz_o~e{k0fnVub~Paf%G63Z_dvAT#A=$bYYDsWk*(PFGB{Hs6B|*`lx1w z3N2By5CIGMCLGjOw;sO8X#wJKYm=q6FLJE zWubp|`kLH*o}RW(sVK3BOE+5xgsWopXj)oY-kNy-UIpO|zY9)7(iPnYX!;4tmG@sA zru0(_IPQp!C#I&V;n5VU3G%h+EH7>dw<%(YCm$N&0=dqUPk?Iw>2S48_BuE?rf!l- zMLD(G;zgBSN>)~*ShU(D=!+&lzuSdpm}Iai0B=AT(I0xxCY8dB<6TByw_cmLEqlEp)s&p85s`9p<*F z_^fsY43w4IMe++CKYd#14`||*p+IO%OjqvJbG$v=<<#2o}U3_Vvz62{Dg_f`&p_I7)@e=MS8HlxJgVz%cV5 zZ#VjbhHd7TwyqHN>^Ms~ICFtzvhV;q1^>6o-kTZ!(24J*rckanSvyftD5j!p#`tf# z0soU<`h913_^kMrHSWk7X-uL}>QC2?FFSP>M_T(Ech_6#=w}Pu=X9VLid9x=z&Ue5 zQ*&L$m*tdtBdl7=RoO2WU*84sp7BN2ZAN3PyAWoGN%^ql6iR2=8bL;HgxWrfjAYv? zeo=AFEd~6)@r#!qgS`Po@$BEPg9|^TqCdD}v>8v}*FY6B5Ns8M2eOG>c!YJx{;;lP zVUFx_AjYB~50sRYxOeSRXliOItQkSm3a4r?*o(30-%!7az-agjf;*(mv_lKI7a!v4 zwo|v5S1KG1+@h(V-~~4DDUJk{5OgG;f4NAO<>cfH<16v-^EY8V+K18})f9@0k2gLF9^}Q>h8OcX3#CiO|IA(4$jod5$S>qE zFxwG8=q}UWqk*H|i;PT$pDZakSp`>oBhHk|FnJbu{2^I7A&=+h-A)`o{`B!ip1yfw49tAOwIDrR6GmYDG}CL?4$!VAA+~1PbR&hba!sHJ#XYKH^t_!T2e*uj zF`l$vVBq(?&7VH;DZ7mKT=N%V1Au~`-yth1D(V8yQFtAfku@{eswhV0L!4a!7l7JLbB_V#)L3 zC{p;ec;MZljKP9 zSBsXiV~0ro!TTy|YC7l6J;wop@0KT}Qf~w(;3FETyW!!9K*YKE`H%7Q^OGY9jOOQQ z|MN;pHxUIUVpV-#z;hb+B!2q)%E~)fsd$<|&yxU}QFGW)D9g%NjdvU!`0@qcP#$Hg z-|gGl=H}-09(i~>KWZ8E9XBmUD>qM57faMFQ#U6&M>jhgGbRs97grlchs%5- ze1beo_ubr_TqXGV?f>-xK1Ua8{z=oFR=5bhlajtG3U$^L`5#8nwL%*d>JhcFoQ$^D z(~VEwUaAv|xZC?yLP=8OGT6*H^>J+scb&bD9?Sl{X$pc zIU93Iy;7jcW6cZf7kri2(g$ZgGk%RxwW?8d;k=yQlD*R3J#%G$_A;kfl9=_dlp?9T z1~wV|BqY}edg7pwAJxd9#o~^1f8>?aVk63t>+OVMk97&h_841*N4A z(qnV9xt-@aFdja9c%3ixrhY4Tl`AVr3j-jx-+U9o~ zZ1Saz6clhVq`8JttH>9jo#a(JA;Oq zF6ZRQ;*!@0q*ky`@ktp=1y>5M^bNd`E!{{-bN4!LmS{#g#)^A6)LQqxKf1`)1lE%q zM}s0kP>P72?`iiZ;(O=Qh}=eZsFz1p>! zWM>&OMc%8ReS^Xxgy^!wtsAppu9nhj-rXhMSS2J9iUAF$#?^3{O7H3OJSGF;^_7Fde7E&{09&u7tj>8b=M!h0=Hb zTTje0n)r`>)IO1Y7O-2(($bO)E1V0HH8gT`*95C9@hg+1%2Z_7OX_L`xsp{jJ#O{s zL~^_8JVKXIWASTH&i`H#?JKu)`Q0T;`keaGz4vLlLuq4@>BKwn5n|JWK}m)ULfM5S zQEOy6UtBjnva@Kxx|gtE`}$5!UQKJhV41?`po7DJ#qZ}qchQDMfxhs0mcDFjRru?t z3LFa8NvTKCXl^XHwHk5U?#;E)i0OqJ<<;E;;>(ZGMLS45)e6ZsCcBjJ^gbmg(+Pmc z)1DV*o*3;aXQp#ik<&he*h^4OesN}kZaxKK^&D(FE!U|mrxagc{93s(uPhpLUlq@%UEIW|5cfs2ws9oR> zQv%!MI?i+@*X86s#KQ9>kY|^Z;ph{{8)Iu-F*|3TP^fFFG7&c!4S}zNiN&8lnTRhW z$0pYyDt4CjbebcIg@q&_Mqkk?ul(ea-DG=v-!B^Ex=#eQ6XnU}$wc26pg+dt=q{x< zOC%4+s3kEC^mlhXl=o6zMQNap_^0Ep#!cl)*DpBKJ9v}mFtM;afFyHXkwV@=^{i|J z;fj-Iw~(t|p{6F5(tUCAVUmoIv90z;ac%=uRFK3<2yl&R#Ld)1cle%O_7vf!B)eYE z^!BtJMdT<>KCw}BHa-2HNsX(T!P)F!Rm2W??-03(a~qqc=36G;Z0|ho8beDp3*qZISg$3QeoG9q zSQ3zvrpGuX3S?{MU=xfUjq}MeuLAAHHhlhvPk-(}L z#gh>qkCGwXZTo$&7)MYhm|zo`i!Pgyv{?RwR1^pI1yQ%$N`x%YIiNEmNx0Fw71Ps`~BpbBcRy zGmVao<>;2WXPvHBwyU^59m)_hKRNh*r^$u=rN?H}Tj^sW|KmM*6%~TDiAv)y0aSbj zL8Yaod(J~oLYXnO6R>k2OQh8ad0gBoG^|^P1Q?x4A-x_Pd4kb&fD^9ZyFWEelx}7^!KkjFKj87 z2h+8_1iY{v56Q^5mp-g)p{!C{=cqh}mj8+7-Z`KfhV7{H$Rc>Zmoe~FmXzmQ*xPMp9Df)2DQs zi!m`L*P&>*`*g?Te5@=x9y@0iEUL}@gGoC-p_hQaVHeio+Te* zXJ_Z7scaM!5{Z-jQ`rcT8@zgHFQg0DqON}n%nMA(N}l&g`OV4dPA?zxpg5#!nSuN) z1J{oxFME{;`-|l}b=JDNDGx=MSst*XbGcFs$}Efne~Gl6{`R$-tjZb8`3~Qq=w1U~ zEdAB+j*jb_dvVu(Q`3q$@cevq9}AZ#hzW1<#e>w&72==UKEA$-dz;e|i$RZIy~wDj zBBcF~pvb0vrGBhF7!cE3xqAlHXm`Lsa493DY%V@*bbmR;zs~!M?uh zk1mL8&vk|hwFjO{_?c7|;W^(G*64RABq=2|zqpvUzwCar`;|OSBTs?2&qAnmc4l|i zt<~uBg4?$l8u;e!32&!Ae(W3`RN;N#!t7rYp&F-~cqa)+hZ^2~)%Hs>}I6B7y+*X#G;%8Tj6g#`hp zxeiT6&MQ}FjEszyD$Z(n)A(Xt(7VRU!jhO}5iBMoq?x$*Fd||R_O9ab-X<9}HCp7s z2PP2_$~?t5T$E~tC^5uHub#0#4pZRyGmMOkb@$(4HZ?UJ9IaPY+fR$yP1oN_JR(h& zS00NL`~qE*4qd6jav){0 z*^~c*@O>0Y>E=yYTicR)KL-asj$2P2u1{2UcXnd%-mU9EVxZFanzAvKhH}i;A(5VV z9O%b{RBJjiva(`RjePLo$H~<-lqdk({AZzJ+nhA7Po(pPDM<^ooO)S++7joXb@ zzJKXY67=5rvZI|@q)Nd5ePGrAt~0=oe?o)#!W5Cf@PWwq5)U|8Z{EogB;^E^b ztO?6mX*7!Me_T>-bh)ZkN$2Yd9VJhkMX+AEF6~nW1-?|-a^0Km7jg;o3>+&Qi8(hI zXtlqPx5H&E0z@_GSHBi!zixD1bbUe3Yb|JG+(+SP_o%rO8h{30l zyLTVo`7${ZI^XG3QyZ4YavE(4b)j!Z=|gw3Sq}jboj9qqwDk7gUN{q;(yd$arMl%& zhS(u}iB)3A>y99CwYIi8AMM(_>gkz>014uX+WzrFc5AjZUjirLOAJj$v#%YgogK9k z+yOUj|H}O4Sm9aN$gDq2BiVY91n|jD9upRf^WQ_2yb#mZCaO&!yA;SuZ8o z8o?GpLBTX(8*(~&diVNk0VmWyR^BSg$vuPz!hQbFtBRQVm**eF#=eHFx?68;X~`@h zakpfQloFcl;S8i<_hDO~r<8wZ;&sq|dVEd0(#R10{B?xLkW_vyY`oV!gzwLV@Zy=a zUp6<(l49e91i=Pe#U@Br2~&(?Fx_2!wn8JHWb-)s|tcvE61Ks5%#B-ezI-UAS;Yk$uszq z#BFSBigZfP&&NYANEX)I&*i!?7(n!IZGFCqjY6Sv)zDCP*Q+bkv1-)s zoY?q=ui;+zEV+tnhyoj3HIyqIr^QMs{{<6O;;k71}$~n^!R+O+Y z1Z`TbP?8eqfzWC#Afa&mIA4FS^i>62lJmOfqQFvEEJr_f)= z`>evk6!k66m3IzZS4v)-GH!pYZ1m9!-0-Z}rW)ONZTc=UBaKF~Hn&t6)>HRJO~>(k z1fv<`BUI4mz*GA9`S~<)XBHBAiHFS~r#;L}j1bMWLVPT%E743;GFD4oO;eh7lUD)m#`Vd%^22Gfd6i^-Y_R|6;9jL)9 zDcVByY=!oKU(Y3ob%dTIkL;YDm28&$+fx1b;Ud2xijP)}xejf{|I+9~hzzn3w!rq% z*Vjh{Wd|JLJGOksMLqNRxz^V=vNgoIUZ@-GRi8FH(>3s0fs;WF#)W@2E3k{W-}zV{ zL4(OMmcX-(i-W@~DM@G8?9s#Dei8z`5f+Nis09tzQg**kWAb`;`NG{ju@Xh?gkGz+ ze8}TUwg~p@iK3*wY(-S>@F_~CR5!=r(@RtkW8ewdTge|WkeLWA?Hu{64d6p^qgeFk zAmvJXY|WN8_0+HBd8o>fv?FZ6 z@g)9IR0m;cTT=~3LcURX)kp}BiapxP&GuxS0J-4SGR5C+e_ubn_Sku2cLZHk`0A$R%sCs8G*!x}>#rIz*+$Hc;7P94BNspONxy1W|AznL#VOjBGZ zONol@hv;BghpOf7>e{{kIWX>KDjzdDI}QrUY(!y}k4lNw?Tgh^NX)O|E3eGMW9B{2 zZxnOw^yD0Bq|wN$yVe--E zzuFnsS`i!h|I{f~Gf#dq8cOI?gyahi8>N7;sbH)#BTcT>sE){UoKAq1w0wU}i+A?y zM9L+)s7s|q54m9Kx59n<-j`2Q@I1FhSP_GU#B@GE0}2b-#Bh!CUPz1Vu{ni-;xld| zLKW%Pa@c+-i>$SNYdfH?bv^0Ne1}36c%+wnV#uJ5hO8=grkCl`e!mlmc{nGEE*H9U zIX2jmj(&bE+7Ttf!cxs--^u!hDc>oK<7(i$3aqT!4Q_}Ma{jKoda}34KVCfz3&zEqdbGk&)^_3L55ovAxc~$eAP`+9OE9EZ~cD; zUKSyK_#Kb9=$Ya@#%|pz`K+{vxR*n(-aBZa1ZEJBDxDU~@k$TJYZe&85;_yM-)<|` zJP^t~8;IRhJ~g*Nj4ZTvwcVg0hOBOd0lu-Z@#OS0EA)wqv2+*f{mjf*FflQa+8s_z zYxZ+(oKjPL$c{)*$_$ybP$mi^M6Y(yGIBStVJN=zoZ-*qgrf;tGQ=ETAADaFG=ELR z#>ts?ufbt;x`AI)ON%(A-DQd3=;wL_WC>iGm`Rsv?Hj|7^O+h8w-WlbxCx0e|ft7`W+} z4HMRvFJJC6T3dX1n_8;PJwTTzax7?ZGxVRJ+-PF1)L|=A?r2p}N2!L_1%3^R=*+2s z+upL%A}|7muCCs1-8@4=7nGNmS3N!sQ-@x~FJ-D}EeX4u|7h(8-@Hkz;*hf_=i%P| zP^5t+l&)GSCMG3iSV^Uq2b&moV0vn*tGl~kca98*tkLTKEu zSeu29kHr6IHyEahF~L>*`a(bq|1r0`_q(Aer%e}4fD{%O-O$m|-9O%5tNg?OGY|yZ zg&9r(0n%$H+o8ktkcXZjBXe8u)c^=%5sVgfW8#Xz#2geA76tLGZH0Ik)Ki4PL?FmUpUVj}cyk&3+ z`Jj>*9tR~^OSTd@NhLqjq3IO{HtC=u@*zVcFj%i7@JP*rMu22Y-X6m`|{YNNYr zX`e0fVhv`vE?XbA#FK`E-4nF7}y_nc6PTb zjii)gm)6$a7(XO8^F?q10Lbg!OlJXznVY!%))ZN!e@FV+&8)2>`udb6KYmPa=GG1V zSMZwol55D%-+F-T9EMZ=rt$1X}X*_cw(s3WbjDip%rK9cU#0`3h=nmAW8m zhaNB0)4P3(H(Ss?{eMEisx2k_4g_$CsDmWj*MKe9m}#ngUdwSi#fNp1Jhm@Q=WN>hhWqRaNDwA*xh|qPVx6a^t_4 z%6Sd-r!>1ie`W-@1BPa*g~i2N`nO7Z)<=cC>{B*6L(ias*5LR0;S-!>=>%Ag7iZf1 zX}OaLva+%yrlq8$oPR9e3O$}_@g6fdY-y2%zfN1Yzi_j(x%m%}Yr5XP8}Oocbv9bN zYvY8l7c0=*_b820M;Bh`OXPB~KywWv%YA6(5|)ssR6O~l88bPO2~Q$UUgQ0=t|k68k@0#`%`trd(I{%OcjRB;W&in5x|V2yijDp{UW_lM*fe^GmLYC>6evMRJf~o zkhQ=;)&kHbrlkH|AEvRfv7bJy!nP9?_EjzA<-E|gs_N?9)Jv%se(t(5d;sY@zandV z(cQ+UMb!Nf{nhi$9-BA({3Kxs^z8JFvgIteiCXfX-R2@CB@Jq96wllQ@Cc^U=uUs= zLVvzN5WCIkh6n>bF?~a1ZAat3e6jHI_Et8OD(zh#-Sx6xhR&POZyTfD@pCL})Xs^C z2H3xF-~DOd!<$qrp*ZCl@KH@v7+xxP{v5s78%LG-^yw?;ETeL{qF|#?A^@2EVf*i+ zn{IAy6r8GQOvnqN^`=~MX=-#|*L7{hjyAQs2UE(OnQHS0ukQ=;-#0$iE&v=Vk6M7X zV(=$9&+G1pvpw+T1g~7t$7bTYTc@O|rgr}H_pze}g}32_fxqFjKkKVrrd$3bEk2&9 z0NMGF#p*2K>uSiG&WUstW(RPK-XFQH%|&4bOXXHDK_l(gNKHpqrX|hHEVH}6Pen-( zwz{~;T5_PYY`WoX&z6Z39qI>^7lR2@kA-rThRW=l$@K?5@fG4zpt z6TILyCgd)R_|x#OYioB4%z=`kgG~k9Sp2|}rf#Lmf28W?_uu|a{SQ9(Mxu#K=Wyz} zsSOMb2PnPh>FMYHWOhr6WKx`TrtZPHbe~O{3~IvUq8r+AT&{Yxqw_ERB>d-g>LiM+ zxeHDm=k$rvnPmSqt{A9XJd34e=7qUlbj&HWZ8DvHlQ(X=oQAOLCbWNjn$;0{nEy90 zNc_?l(+}#d@u}$%*~4xf2j?PCb@Kjp-FnU)dd|nh-_}=_qImPLa=w0Xjvts@l!an} zqV{5EUba~LcInqUj1Gk4G|W^{#@?P(h{evHxIJJ>C`u>laQe88s1EH%9!`jB5^fB& zu>|PlK{a9%x$cC<)w$w*gDfW;s$7M_|Hz3y}FrCXM$Oo*#!#i=76#E*#o|{8bP~R)O!EX#0Cmwhxfh*gxXb%Fp$D}@l z+|)t)4{?|;>4jORa?ZcmR5fVIEZ*E)qW*K^!NIBA4-544&Pj?8#8y61lSIQXkv3{W zKAK8Dn)2?UL2{AMQA-F@NB{8oGu@%?A)>tp2pUOG)MBAIHBVUjGlHLj;qEhi?HifD z?re$YfBncC`{>cL%}rj182(U*!-Su}d?sCVI0(7oz%aJEcV6suYBxQ^Y1vsUe&q_` zZmK~V7UDH4=`PN~CeAPD*!KrM_Q7qvV(y>~B zpT|BL=`XkOZaSU09!O&z#GD4(FHvwik*&r>N5ZtBh6IPprV;B|dyJroIJTZ}xC68; z-PB&i1_ZGaL;z+&l5^7hZ4VbFEJ zROj@iFM(x%gN^M=rJL^RI$*p-9T)VZph#WjJo_t^JxBgaz~+!mfKi1mnFrzM?vZVm zql#+L-nxMs*GY@PkfA(E{f6hMrE=e3zpSnU1B8p`3;3+T;RPQrF~`}M?~A?W@QJHa zFD^+RZ(QEpoTj>X@nVs2dqBdmVWSg9MTL5ZBCu+ga$o9bDvv$V*P7%l+ZKA=k(3`Z zl>hwu>(6cD3^=Zic4j=w*Jb~TJq%gf71%}A4QFB#s0p^)yXN7#Y|hK?QhmqT-TmA+8t(0$*h+s-c1|oy8fH6h8Yy4L-(NwMnl6+VB!p z_?+4QtfhdWX$mU=E#Y-FH4;&W8MINW52QhiPtBfUjq30_7Z;bhxXVI2vQIUOVKC*< z`Xq9k>>hZNJ&MKXXhH9(T5Dx(tG7MXhF|4nJ3Bc9PVpVawD;N?Voqde=Ke@Lmo-TC z>q!^}N|f`U`?KX`4o?OP_X!*M>JQnjt_lA>kv2Bmrs!euCCw1Ek1Dv8Vas?17k2>~ z?mJZ`G6}(+00*ICV?Rs>faFA6LLwz^8QMOBro9HXTs+A)SZamRO{!}WQPdNfIAKFl z8`JxF{c0O4`c!|p{U@&)^o!qtm7+R4sk0d)WnifA ze+uJeD&BwyYd+gwaf8t zXAa~3!q-Ay8NP<1fjUinw}W$j^+&Xl^$B^tKT=JiUp6N5n(Xk`-#41Z`P}*`=S^%Y zAK`=JD`H|7r?h_H)01g9^KdJ3Q4nBbLtDW|^7h(ki-#|C!MG%Jch(jG`T$Izv~Oi| zW5dMSy0F0zsKevbG>TqYE`3t47SOcdMzsl7G*jbq$zr@T)(NABHC{c=vHwrXpc8#MKR>^bTzSGTu4hchzAl*m8f8DRb6;h}(YS6a4Lg8EcP>&+ck)W!n)!9OL>vM}nnBsr;^S z%0qa&dsjtv>Lrr!Gu{2eDW(ks)zxHGP&-$0W#?CZT7)OU2LT<2238K5jKD~o2~HTQ zOXS$T&J%<90ihl}Htw_>EJS;)mp33N=0%cQRMkusan6+jSt_W znv)0J`VxCid0UE_p=@lrRHJGlwY3#8U%HZLxE@E6>0C!JwYU=#U~*xI0^+ju@GFR& zcL1zE3=5kFQYB^0O`G7UfAE_dmj~yoHG%K#Y3*ulJF$S~l>3~rj~C(5QmQd28Cftg zsQ@OICTf2HQgIjt7Ou(Yb7jr!SfY{LwY|HKi^SCQ#!3gOZuXEtH$ZStoZF6g@5t7{ zVZEIP-&Vxe7j!Z;%A?%~g$u2t*{7H8$gCm=Oo`o89ez$u&eUu+U`J%3xWru{IE2r| z*=agHQuS#Ix3+~<@(&4|F*1hHyq^uf1sYA+JcUQw%NdSluh3+G=@uBJ1MQy+vxw%? zSfaUY9HAG94i1X~XJGo*PQ;!KJ##h}CgPqHXLombz;P)G^DxDaF?H%f#2P3?BDPZA z1(RZT$PBN?L7`Jm?4YHgiG&%bduZsEwJ#uSxWpG8!o_K36W+vhQ}xY6(JntrAC3w1 zg4)F%&$i9M!NDRXrswGE;UOAJcZH?rBHkhMKCGw4L!%KKalc@0qj^)3u*#biGk{H{ zJTnO1hpL9!3B3b?6wp%?`>W+^ntpEc2LXe_p{UHEq@f_dLCR>ls_r)xU3vCTF|TPu`bLwNmqkfjnE(&EUC}Z9Oa==TbhkX^ zxzMYOcYQ}rB+$+XH3*%Vg%&@@@pBur@Svm6tG3Mc{nk{IRiH9&@Y14OJOY}hbggu( zt!FZ!PB2fuFVP`{C&^v&lnL*4v9_hut>lg-x8jmHy zwtmSL3rP~=XC<-SbkI5GPAWOaS5*n{+^bi0JdUEgBmkhtMKx0qISn~0H3lDEmG}6~ z(8!2W(ML&{5;beTprBixTUjdkR6(;m5~xr9-u7LL2pe=`YKa}qkOV<)spIII%4b*w z6Xr!VjjuK#4$$e`Cu=gksyopTo+P>|@$q8~XLJeAkU)g%yBwd+hKXD`5G%=0(D=ay z_DUqOX>?s7DbXn{X!tZfPTAo2Ic0$&`9cIgX);N~6;iTqNUEBN&m+VDGeBl&h|PRMvjb3)+-Y=TPT|pq&BIMO+;*Mq z(SnsLG|l9D4{^R*!@#1#rab5WV@NCk^gf8TrMkKrg#vL#+1(cezzv2|6jmu|;A_O= z3jq&U{HZ`UdbJ&PEBd+Sdw~eZRx|yBVG&6oNTVOc{;|$tt974Y)^|&4=A}Epq9jfp z9w}=h=WlZKk%A}`l{RI`Ek{OOw}w6Z*{_JVTQEXybOie|$fHJBT&|&>{VK~oxD1<_ zJ<*L}7IrPDUkFLDF@UU-hr#%{WJ}Mx-3^6TicDAaPF1b1r9E2I!re&~NO(m@ zDOBe;7Ynrpt3ziB9<>DoDTujZ0Iq;Kmh3SulCByqHtU5okjftcfH_5BwGs$QZ=_p? zRxV7f*0fSrU3;HD=obQV=0n3{Xl1Ta&)4R+x9)RZ{P9$6Mi?q%mAinjc9A(g0>L221 zC*4<9_dIBUe%4PwC;*A8`LxUIf1zwx&apD#jik2*qg5mRywe$}A4EiQ78YzZwY4v% z8LkDy0CNB(U&Waqn&K}*1>{U8NJ3FTG%}YFsVx~k8TwX?h%0eqNo;QTmHP$!i`%#` zTHZ`#m<1wrA>BGlrq$=?NWm>y5IcZ$jTj_)K(e$WTjp;?J`}6Cm^`)lB+C$HD7|rX z36GjFJl!y5a<6;!c995|h{&x##&;I_VMK!iMU$J3j_$d5nEYSiCG*#dh&HZd@vYqqwa5vUpOZS1gR&Ke;pq9(vHD_^EuH+2osz3_xt9n~N~`eP63 zPhSr=OlgnV#u6#Doot0b_t}l?tP%!g%u1E~8+LX)Yahx9t*xz3put21fk4e{bLxfW z(-5?hCbD^o)*z1uoP6)S(;grN0PC5s8#$k5OLQ*zeea<{Ue?Eke9OKgSdsRlwM&Tj z4Os#Up>z-5^$+FWLA|HsOu)jDbkQbFwqv6lHxwCha>;WCZ<|BP3ug=72Y$?Jt+;Tv zzI5C+ld=yqLi!rrbl;@i)+bYhjDM54zv~<#hyo?$2;}9= zO?VW+?d|Z`m~vgnYin~-28IkDsVqi#V#X(zc2&6-Cb;|{L(QEGMWp!~hbO81i=1S1 z&vsaTLQbZk4D~Z1ApvP6oqrv<1K4Btxs4vwkR2fR3WK@iCfoyep|2x}peH4F{tFbt zbe~|$yplm}Y3#j^m~B_6gN`P{D;I*2tdpuN-I%a3oZ?+E2;s}J^~Z-JUBQp#Aw=Yb zEdz-T8}wjtNF)Lum9Rh|r;Q7^gt2Qh``gOR@ zdOUC6yWAms)ipsZ&bJ;D6;rs9`0Y40Dg2_lANm+fp|~+C&$UA8KHA?p3qJ^v?X~%0 zVx->w9yDfhOKf?-yPN#0ncDjYjgEZ&%3nokNR@KNoIPDL9)_ zn`{Y?C1}G#L(MHOhkMR?pV?~pK>{g=(Qp5g=fTbZ0v-kfYF(@*E2o{KolAhQxcOsj z&-t%D|0I3ct6$(DdxtIH_o1~x zs}ChW;+okZ=%OyUpJ+@}Q7z-TR3Uz=ydXu0NEAW$^+TTsVjRi?q3k7$m8IImF5eII<~LlaaF!i6ZR2{?Rw$rtc!1tN@t~JLGs!oXihrUd1CO z?y=7HNA^p{?5t6g>^U+W<=1;8w7!c5^Q40)Rk;tU(1pRBQ5kt*eEarIlP(sjmWDR9 z=<@5PF6KrPl+^({%L7)Zh@e7yE=~lJ6mDB9`cf$f`e$zhM0E}8VbNxn3{zD@{AHJJG3`Vv@sQq)yd<*r{x zZESA7t1^`>GirSqVv8u%Z7{2nGI^mUGqZCbT4iWH2msT{ta-A%PF2ka2n#a+E&5G| z#<^TizEYBsQNH~@0Vi!nhnMs!kXeaZ#FnD%_t67jRVd6~$H>5xCuq?}dbtG?nwWvK z+0P5%()TI8;l!$-Lsa8wo_34>(LLm69;cy|xX)90QT@N7(h(=jlC}Vf_VTVwo)eyK z=@;}Baze{2Vh#_IhytxUwGUlEU6j<$!BXP$pJOV6bJD$WjEYK1=;8X2k=sbs1SyM4 z(~QMW_S_QX!Qt?iQEd(Pt?r!8XQCu^O8}eYGMjukrqpz(RM`SmH2QB=sazn*$*B#p zNYzIxN6DzhrIG7Zva)a!0$2d8R=7(3=)KswIimp}0S5H`?0F=TiynhoVd zO88KnY;t=-B9qi$#GLlZkq99cYLcKHq#M1nt-g<>9d&he^-FHb>BPw6%w7<&z0Q&J z_2+UyVd47E1>5%(hSMhA_wL>E^75jdv7V~4K~yl(!+m|=B@(-FRo1)`icV|@5Lo42g(sE>>+_8 z@h9r55M<^WTa*8Fb--_zhT7WD%*@PLk0t}7Z`YZ6{v|oM{RjD|;vHayO-jaT#k})X z>A?Dc^`i*QyNEpIT0u!)S?7a407s*@7pNMb&>GJT@VCD@{IMcv+J*hRpuh~on5Z3J z>(5?`#P)}vBUs#OKUMJZ5=ROPqA~!=sOPzI60kwgMdIODn~&_S4&yxu29N>q(Ab%k zr9V)T`_Px`G!#sfkoCLvnW{tj%FnHrX3NEqxjhFMu}WAn=IpcNh$mI$| z-$VARm!Kk+U8xAx$_wFm#LJ@{|!!T%4m2T#hO|HglBE8;NIh(Sd~ zg%@J-Z4DF!;5q&e&6g=72^r1n4#GLmjDfjD6l7%Rq8LO!1M@_B^94DJoHCYe25CRU zFVb7ARfv>#P#BrO)SN&2`9Dn6#rqkq79@7qw5s?|Fo8<~jUy8q8@5Xmoomwa4c-#L z@@ZjnV-gIXO=Zho#3ZeGSF_F<0N{Uls_)3p4Gj*;0f{Ey@nxBo--m3 z_$K)NpfI8WsHuGW2gqdcQD}7Bb&m?PXo)CVB`x%$A}iV0Wrbr&q&rBD)4?_}U;6Te ziIY?DSR7m;*8?0wLD#x=N2^&?9^U0hf;&fxx{i`)^pCh09QsK5yi|_O)q2yzYTp0J z=f@P4d;gn?k}~N-^4@I)d3nT5<9H0iDj3M#|_;rvEEflH2z!wWZVE; z%gB3)QqpTXNVX4Mq@+_5;rY*w^ux?9$iy@Y#0K>hr4>qkamEu4Z~L@hpy0QdoIqH% zB*MfAvQyb?X@7Cuny3FRP1Vx?lYnn>BpX}m0CQ4$TSS!#htSolS3#`z)joA#NumAg zYtTF|D+#lNvd(+HWlRd}|NMJ*^pm)VKbPZkuEH8W^*(_=4*5}Tbzj~0`!W>rAnQ5@ zIACS`512q592|-ao9mS?Yb6u4XhzH=KCet{Q;R2St@)o!cyu0SoORyMVFg!? zf@&qras>x8aY-B-9{|MzBjd&(QzDqA`rf>`;o~EL>KGUpuvn>y>Ns9~$7=A>P5Jl$ zX32xihR?TKecazy8h^G}0pIAW-M;KcGR4|ANQ1U!zXjFTl3<`*cI42xePRkMxhksp znl{}P5fKqhrLq<}0IH!-U%!3@*G@`*xESm>DVWf~r0cZkvak>jrG2WId9NzRK6YYvF{K>)>eXAT-5Is-#e9%?6fGB4Ag&d;Ay28N=uQ`ZAA zuAK^E36SX%wxyD#E6AVN>C4Z(U<+X~0pB|S%kszi*Dz31Z;$XdX&d#TvW#x^@!EIr z^04z_@v`hcU_1tgJdkgblXv#k5$)-EPsamH5NdQQ=S+-cB$w7q7QX>hU$>&!V zv9s3GQ5G8?pIsS~y_h z{2PTeECoeHR!DJY&N94$DcN~zRw}cyL}6^K^j*LQ^;O{J(yw?^9#Odj^bN@ZsdB`B z^9F#CKBkB==r6~Jr{+=bq&=B(DQ$WZZ}Qyvhk&_m|L~~^F@y&M>t~T5g$KK37}Uc((fn=bz|&uP$9~${kJZ%c3bF{#^8}@)QXvkX-^W`ItUm#5e&#pG z1Xa@nFhZENlN2v;dY|0_g=BTWFQ1m*N9&r5FGpRpSBGbwef?U7rv0coo^kK)Uttj- zbO5%FlDJkqo&}wb7P$@!-=k)1>Z4;LY$jCdYvUV_f<;s%T4c<-!-)|=Jg5g^KmutF zR3##s0W)Qu-d?J4fJ|>J{dW+q=YS37HWvkM94K)k!HHI9J5B)q1kFtc5TtPkTrVn` zzonOPEl|2ZJv+9jh->3>JL5SH)$5?N9Vt|21VmmY6t}a#A0GtBeSn7`K3L%F0XC)Z z9y~bk0wD^sv@`>3*{&+nZUpm;xgbIS-UdY|WZT16w@ls_Y2;UM>Zm22nFS`_?dz!g zTDPw46QkL#&awAmHO1q(!|@>@+&+@!90%VRXrE{HMXw7shW=Gq;SgKa}5N6%r!<<3K9AQ;i8bP?*~5!G}RcThKCSY}}4umJMH(Gy=C0 zLMT6eOi^d|vF5$|I;>HFS|$PN{^)c3SO%#m1V#l!;{fPtr{zH!gC-Z8+1XiC5U|L( zuyhB2=fBI5JrmmN=IPlBEFtJ*N6Pds?{0l&M0S0VMjn!j!GjHFvplxtI-}!|c-96{ z8GVNwq2fQOQfZ>T(jVvZpy67x!OMB#C#Hl?atK?`v#&ig9EuNnoW|LGo?F7g)~8uS zxb+j{kzr;MISY4}F&B1TJ)`n_4IjKW0DLn1@S+SnMlpIu-Z;dxq7UJ80S_)YusCb< z;u8_Q_x1aI;*IJ6Xk&hJvz)jYac`Xw6BDzikp;0tPHlWJMP({CXMgi~2;OR|p$jLw zW9s)my%hQ4(lq=d`aEyq(c@&5dD@QzZ%s@#MnXmh>*nrt_nQ^qT&id|uGY+V9it`F zuq@mbk|cUow)CcSd&>e2UU1+O`w)bI1xn3Fun*~wvuN&?D}b3IA>-a}(#pe3_Nd&> z&&Klh69PT!`EQhW+?-*JH=lh6ht!&+^|va^i$JhJ3dXe-Kt*M(*H?}5keEhZW}(>F zCMeoAuUwFdLuC+BBFDZWa18FqMjD- zABgfE;51WMEoctTqMmtf%_2_(ajWs@*U0&f-F0#jm}+n|UY-Dba44ve7xPn%LNYVy zZ6ThP%@Zt>{D>T7PEUTEtuStPc%!9li{mSyWp?9d!4@~Kthp;aIFAlJv|jchySkG z(-D|yQgAx)O+1K+jO>Ku{0^*>o40%{GO;xl+~7D3!3Q6Bcnlgz;~1ncQ4l^R3$G#} zw}fkGJ=T5lBUE;G0l5mp7_uz9Vz*`YN4%iRAKB5r{w^FSTB;)HzmQFoC zqPZ0NMfQT#U|KNLxrHQkX&E39L0gIB2FQgL*4B%Qao19g9_!|B+gj?#%zHrWrgPjDYl1*`c-Kr-nPRmaED%cW&$v$6+fUWWfDAH<8vd%9g*m?3v7+A19$6V*icv;Gl24sCWhsmF8(D zZN(V<55KR?@&&|zY{}+5mnr7R0PI#A;uQ`K20;r36qs&UvnudS6{}m5y?Salu)Bkc1UQbGf3_wVe2*qo=Sh4ZH)#W z#UgP0Q_97WQRe*I@4kF*N1?(d@seJuUUmiY9bF`Jx!(;18KziMtPtO0ujuzC15&6! z+EcaUCcjj(J<@yU*T)I3mxpqw4o<~omVybQ%M(%XR_;qcp8+L1Qe$~@a&i*!KlG&7 ze|q_XqOguDgbDFt->nET*+S8R8!Gx%OwZGEt^(Rb9~?5=+0MJ%6NwN3iIPUT61f*x zHKJil=Ymw}{vWZ7K;zvMCOm`MCsEJSADMbI;LSC#PHi%}XGEeO#-W08Kn;P8;Q($B z4kPeTZUZ7l$yuo+TVL`R)ikholPp(tG z_yDK!oc+kN;Js65J;YwReZ{e*F}V*l==j;dVcK_z-({dt(Ph)TyVq;L0FgF%7ovWTPZhpZ2Cd$dyu*WuIZSg^#~=_OM2`!ahPNp@_&y( zViFb9wQjzsmjLum{az{LQ&Mc1W9YzhoH4H;$&LWKCN3_HM8M=r_tIf$Xx?Sv}+SQE4!ri0vf z=GGHo8C6vx`0V_N^N8IMjTV?xgTq50Hoy-q1I-U&f`vYE9xgR;u>qJ&ESk;s0tE)GYg1F<%)WCt?99C&}GQ4k!NAehr>f@gqDOW0&+=ZTL5{5fzs@B%XX!hTzKfw}rAKJOoww&?0OUX{ zr@35RLqo*x-o5*0Lxwu6RbuDlG>7upQ{NN87=VU-o(qeXChaeg<@@c5L5mj|KrhHC zM@Y{Oxtxerln6;$P-Eui=W~cl!sQed&1YM@)1dcF;Wv&2kpd+WV8}@T@UQ@X!Tx)a z(L5^~v7C^yQ41@EQP#w_N8~c3^8@?TmaI0V= zqV<|6zZWq#X95+xYO)?36C($BO5IAOUL}La#$+&}qlA|6Mx{}!fa|iV{Vy#|O@YAQ zCuUe%XfoDqPz_+AtlnllS{g`Qgca+&{8lm)rXMU6bmIB+-M)uAmLN4Zh4YQ3jx&jV z-qHG~l)@k!n0a_`fT%t}$+pCTI@nkf>sYx;+W5s9Iq$G&265tt8-}Tx?qxW`Y*G2+ zPQKM zXtVA8L8x7%)dp|BQ)7V-p=Ke}EvrdTadqy?7i`EX&kGBw^dsOip6OK@-5)QZydf(K z7U~c<-2{#qX?=C1)z7xY7abGx_$Xdm9v`)_G#Y2m3c5v*aNAD~=`6u1fgj3p8l2+6 z@xY86wgB$*XAo_Wy1T$2pOBI9z$x&>U1Ai{ID+t~^v6@HJLdBliz<0VzQL$ZA32F| zz9Z^~;bD@i9veZ>GJ+LA0J#3V_1oJ9;cHq?OmV|x+nDgCL@Br!vPA%OusUzOjX z3*!6)N!DfPmj<>8ikOo8q+3AJh$v5xLzQOFzCldR&MJVkMC*7BRxAseTp8nIxZ;A1 zoG$U8vK5!<+6ah9vhUBfGZqMZu{=8YCwrzC5jMu_2I*L=y@k%Zw7qLPn}y`y|eJ(n)3oSxb@GX8a=ldkTXWW z8fvqi^!V{}t=~`yVe2a=v^KN9+f}6s*o2`U9uYDW!xu#qP8*ZeSTGz~N}QEqKI9>W z(CUT&)+p8GMUESQ<21Inw>zM^gthq}tN2SGw`aAA`y##bG6Ju{!3WDbAicElrG5)r z$n8_h}pnwIEjLga`kFp8VNOb%FrP-0TjcbF&5;6G78XQdBN;DGjmn{e9sqI?4D zGTG>y=Mg0g>dL2ATyYV36o4miR@nEo(#kHVaB$nbA8W127#ZW>5DerLxCs(#uHk9N z_D7Hc=8==!G926VOC}i3oeKr^@d9`ksRhljVJ^tYd}Ib~G;(I0nYlUAWg%;ONw;i- zX3MS&l*Wi7c(#?c!QoRERI_c!WafJt9}$%f9JEmFf9wH*&C8rNyxrFVf zC}3d_yN@&M3-FDYLFsP%ADw-7JeO_X|2Ik{86{dO8YB@VQk0#BB0@yA$jHdbh{ztH zA|pbitWe6#3ME_7Fhepjll6Na)!lR7_w&1--}vKty{_wZ@pXRB^E{5@^Lek2;}oxm zb)PF%QykV8u1z{wQ?{$zzb@f8D}E*G)%)$VGukXMlBkd3*1i^vu55_*k74hYO56+)!11hfB5y`de)ABw(jDUT!sU@(>B_ z!4=y%yjF281ArZQ*k#ko_4|&%Ma_tS1FZLUT)Px1jLW?>Y&%bJdjCrg^E-2uqzsG? zw7g9!o{m2iHMh2(?>dF@tz}rEZMq`s?PUjnuJ5gXn1Kh(!=JZT*3^{aDYXCA4~0bD z&$OHFxy>F@IfA$1?h9@b42ks1lU@?eaE4euDj;fx-9Tr29Z@1aUsKz#f*98N`X zE=ONkLZ~UbI;5K|F9J_W^e`w+g&WPDI)mP^pBc{T1ljak&X;&euw1aZFV=$^v zqM4laIG_}&8W`19LQ}&Q*RkdHmOi#$P>^U~b=8Td^*s|M8?W=gA^83S1ihsQv zzAIlGnLp{EmW$thB&#H$6g*Z#s042IhdR5EzUbDiYX@Aw7izr@tW+4bYSn)Z=k`X2 z{0ybI{>LmypDPp0ne=~<<*?M2GYu&-P^)Udv-okSx96u8-%v=VS}r$Q>bJb7Uj72I zE$pb3odv`b&ud=->Cig3gw=2h$IXcDYg74CpQtH3Ffk7w0C01wE~E^z;e6d)F79ZEZAmB5jfq5*FXy z@#l{oIB)>vOC8Z5F|b4Hbu1zCgiY-?mFBtr-W{VEHY70Gx;U^eEL{^O?n1X@NGk3;lZ^Zm%E>hDgytqsGGt3$CUGqhX;51 z(dHwX65al$6B~T#S4e{Wb;>)G9;>y z|8l1kT8ld`M5NC_)JrfEH~_6w5>$5#a#YvUqy{NpH2YkzGVEBw)AF1L4;~cT6!rdF zQp@)>rx#^1uj`)b79O8rPnuPt7}jy`{pS$Nw_2b0WtSN7zMq?6C`w(Gbvb}mIO=D2 z#iP{wUYB<(pMLkDq`>L$t!E1_#ER}a6v=Xq1=jt77o_}5V%;*>@?pO(A3P$0uD{QM zzwAR(`mr3h-LrdV$wqX4>M`9J{a|enEja$S^J-~C{ulr8KyN}o%IEp|>WYe`h?OpK z9L~RihEnC7y?+zf5qRd>aS&zoNy`B|L*4-(y>i@|?`WbrJIj1YyI@cgxg3Wa@m(s$ zD5hAX-RAP4aeN6xDe=jN3%`j^6h=DZ^`W*`J``(pxUf$-q+YojmrAvgRq%Sn*e00* zDk=*9Oh(SqLE^aU=%k{&s(Mq>tu^XsCn+h(<9rkkooJg~XE(L3KI7&;6xFoRQ1)k< z@ProM_plpO4wF0Q22}o%g3mUZ9bnGmjcU2%`SShy*Mbl8q!da=OOCt7&sGFp*Fg5H zF5-#q2PRBk4;W8)@IV*YIzJ%3Cf^FR8yf+2&@3X#;LZ8`W7~J{ev{nTF@eG zn0-K9{Xn;6m-AKs`~o?Z;sM2uZ*(5Mmt8iV5mR8UyEwEECIX z`}p(n5`FihnuH*bR2RUbOOd(bODd6}k1xKYo=Z6M;)>~$NSMSrW`+xes&oCBn3=DG zYj}el{8o3vO{a+_6>+dmUC<&ci;Rfsj!~Fy^sA!V~ZWqEO=KpXo}dBzyTf)jZb&q(5CG-OiqhR+7^7n zVC&p`$fwI%tW3ehXHduyKZCTqnoM~vpeG>3du)d^2oyq&a;yS=)7E_e_qEi`&;yX- zJu)(q;M+*0YDXX2+6xaN1CVu6L;+Rs0Q%rb&*lSr3Aq&@=`6q#pcb_;JBH@|zY|LK z88!vzknz-M5bXe9;R0-Q&m1NmPTC_O5(Le&+}z@l5^Cg7@qTwYeP#i8NgO)o^ruf{ z(S?zmvM5WuDH5JX7ek7iY{W*^#Q5Pbq?QK`mSO%`Y;bIQKD|E*=UAkSSXfvPx#-K6FN44|RmqGNIif2&G|_m* zp{>a{Th5&Kf1lrEoY~_yyg4^8(#6t;cMH{l7E23+6@^3=a$5#f*A^6q1cst*PJ5?Z z^;*2)iR0H{$5vC&z{fN9Wg2{;p|pl`!%L{ZoG~M%JJ^LSU|~Zb?*)UA6R@#dJC-#& zaE@6TO7putDOoOkA_otaAOP~9?NDQ`+>1RMHf$h}x8d99GCU89zFKzBh-wc`0F;1y z(-V)E_^imCgMo@{nBP9WU`6O}X@sB&F?mJJeR&%b2r7yQGKqH)!p_s(`HSJlf$N(y zGAiowj}dKj>K7$7%*^J4ial1ab`l9HYO~GkjvalXHK40x5J^ zcvM~C=-h%>GZM%*KRfnTF`u6D18yg{BYX&63mo-bp8Z~|p%E0!p}C*Fa7+hz0;nJw zcL-mv)lBYI&jJ6c^|j$5HVHc0p$`Nx!?h787B>@sf@0;#>fO0POg%|@zxn>Cz{%a- ziH1)vyb~qS1fkj=QrRO~KEoE~I-Bc|J8sdVT%TFLM6G&iIz!Rp1;bdc!PJ=LocW2F zkltO@p?XL#uh-i1maRWKXQS%{tx>j_oSYnDyFuCM0m$h-tcJ@F)ODOM|2iI>Nwq(4 zml+qN*p-%-ccM%xfyN@c4aauQQf3NS+n@qF5$WlH^?9CnHv>%}TpajPA_pSb1Mr`dZk$qt12e;AVSZJc zM6!|&F#Ep7wUx&jqJu)wI>EiXCJR$EMB{gHaH@JmXni{aNNMAz~Zi~3iAb#ayCu#@m#@GJT@b=pNB!- z%i}rh+x`9gO5p?#fPoq@#JZ?+0jN+Fz{7+cQvGW~KQhw%Cfx)DJ#I^Y^cL|r-1>AI zs#c_tCBICSDVEHRnONQX^B2F9!bme$sdp4?kpXttpQ5oUdM z8QDgwA$$?(<-4k@frKQuA$8JI2|ftqaNNwU3au6(E3!jk%j@6R0008*CtpU3`WgBA zxUC3Dq+?5P}yAmjW0-GBXW%ji#Q@4=>E8HKs|9IZXq2%F>ro zxQ-w*G;Xy(vIUwxZbYGltnnfe(B(LJ31&Os?^$SSVT^GvF8s>9>vve&jmUumo)zC| zUBQ}?ytclF$~$k>SMNmXgX)~#hmcUw7fUXB+C$T-!x5xMMe-D}Tk-3ZE}O-s(ASc- zSq9;I5aA#dnN3bkjyTh;J-4tF@SkQ}?A~^4FN2H1u(xscdNfiZ#mDX&yczvGYucB$ z1aBDd-^cs-?!Ou{`FdJ-_VyQ> zHB%x`_I+}iIuAsmKdLbEv5v{j&o_ZBzyKsz7Q5SX%j%KF`Ue(feFu)Xn)S^MpK~y- zJj19L_Tzm1s`Trc@^7Ap`y|B zU6tW@Dd>vJ@gH4j>2c-R2Vdz+^FG(&C#O3(g zh54z&l{Gf#zVj=jC!7iT0PaWzaDh+crpD^(63w^2`vI2}{IibnTOzXa@y0x`64vlKx#}`6)79B8g0U#pC zs=nB5c7!`8oq0fy{n@&F`R0RHPPEY{)TN$374&s@N~>5k?u=OCU0d zx1zp_H|9`w_fB~^1CCpb)fba`s~S6AjHJ^a@9){+6ruU?cWw@+t)PipYOH?3ini4( z@q3$9m}5nw4c_;=RbBO2sML?!l^A1gOdz8*K@Mk_gBotPC>bk5G=F zfFTPOOHQ}QbrIP=u`TUZVkq9Nr9&v9X4SBtK$x zc9k$3mIC2Uz#cNJfiNc|#uErem~pgvu0{Y5G#x~z!CC&J-JO9%C76!P0Lb{>>B|>~ z)k{L%oPTsIB~Hb|b4ru)OR!%Yau{C*l>kv<0(tZzs9Rd$yaed7j-jCd5HAvs&l2~Y zzSybDVzVQXQZh1e$l@oYAinAiQt4jcb5KY;5$*;2$!`%s3FDR#mNGQDsuFxc z{oKYs-2P;|Kg6P4SQ;>-K2bb!!CkccR^z}fpP$+-%w4UOd*DU2{@S>lWbon+16+hK z#2=Ta4TTWJ6?lpc>f1Zm?$M2TqH5O#a3>-f)kE6aND&)apkX*N2rJ|}JKj=PM~Z~< z(4A-Yptt!ocW9tl8U#R=DA8F+LAXZ9DHK$B*t^KE0Bob~Is+ZJc{|S}wuh-`76Vdt zzjtrTRp3{xy-SgKfNfW2es&tgi93W^@B8|B zXs4yy0%nW+knWx`ODMg5)`%f3*-($`&9s)PlSW(hO6HF=z!DT%2Qr^^tWcaRV2SLB zy^vIhe06_f)Ncw(#u37xix{(PBfx)e^qs<;M7P z-~CRca5$H5lUsIctxmFWa-ZPZO}kDW-YR*SC*k&8>vdn7_8I(ePWS ztkT;_g8pQ7Efe7o*c_5aeZJu8I?x>@c=8$6{fZ zMQVF4+%-9&(~*&IHfT`!>Moh7I(v$J>OpvnDy`9T%Z`i+CvRcl>L2%b- zYwh??5$;PJdWOE3FVGsp)LuDm;LJ2rhad$gII5i9pnv;4gCMEpi4(6#sBU#i5;FZF zoP?<%eys7xPrm~h${NL=6B47Nt2dqr3X31HEK$mM?KGxy z5iJ$xLG?k48!xS9ShAC(6~?Gpg@n=*PWsA7#k1R!aBWGf5ilw?IVi~Pqum95ezZEY z!Ai}NFWRH~A$R2mA-?tZ+fSN70_byo;?3!0yaCB(ueWmC^Z|TF77$inx_&jwprSjG z5W$zW?QUD((fb@Mu}O6Kc6ys`S&ZqZF%L~Naivb>>^bv^A}h5`u#$QhFpVE@oqPDB z7%%`j29X(dJz**%b+Vh~oJ1aW(w%&}eWKR$(TA!A-wNdx?tp|(c}8@SzUP?fi={hk z0P%ZwUsK_|-G!~3bhPp@Vk<#ExnKy#*~u*eG9CT7@f%xD3pc7d@NH5#pR)GWm9e-` z7??>o#;Dr*eI&0Gczq`lsp zR#aWMo1e=C#^6Tuvp%T2IsmVHNj(;>dTwqzLmqbHGR~N*aZjDFd)Re&u*QqAF}ZR| zZjg6wRDbdKP=oLK2$d#9N&nau`TF2r8Qd3!f@i0)FNp^0N=wH=2z)cTBy@srMF`KK zI+UjQgGj8Wqu(sXGtuvrMay8yH6THE>(Ew71=_+%H`H+c0<1cdx=GXe<1HB4wCBa~ z#Vk8&t}ScN@YiU|Njv03!?~l-DA;8g6Y!?4kG7ws=YCFTBS;oaB4!YTT6NwQ$E$-3 zR`su=9@P43yR`~7;vg!4RzPz|ca0%PFVH>rx+@+h{k-kjO`VNhH2z8Zbx#dmSs9s1 z+xb;tHw@T4p#=t%+b(aAs!+m6w>!|dzw3(cR>b}!-iR&nu!3qQJU8XcMgPTnsG>#! zhE0Ba%!s}+BP%NQD70mX@=8fhx@(HbrRb9h`#cL+hj2`g?wAZcR2G2_Xn(c6zQ8&% zF()LZ0!=pbvNtzBjr*##{^rz34(JvXKh13g;9f9a2(KLnn}>>)$>D(qio0XaHI62c zTXq?Vb$CVpF@(zIkwngxmgO(d?yJA?H`a3+`dkuFJ z5FwZ}0|^Gm!)xG3JJGeZ_9}C)K^O>1E}|kPsZph;zVDlW7LgO0Z7P_(fe9>ymaJjt zQ%I*VRbUx>uYeTFfVkL1^9jdy;A0>aAv#%XV&tR*g=FMdvwGDk;{JdGf&y_8<6>ye z7f}$j?)~4Onksaq{)FcRGAS=FZ&Vkz1QNLv(L!)BV1{9vpvHBOG63+LV{pZ;L-FE! zmGlsAJ~BG$cH_p4QN@gmbV!nz0Kkx*ckgZ> zVnIksR^Y`IKYf}JFK*&Tb8HwyAA*)5-DSaW;6LuU^GTgWE2qjIs%ZGNV0)ll2C9>hyLxvW=~1i zt)TbYAViO@UmSMe496)mzl{f0k&r1eJK)d|+`Z|5VNkc=_g)tfB!_hI_=kuKz?oVg zQh9Ky>o6i}$<__7GhZ ze0D>*y%yktU-gV~1U%e@rWt9*UV(uXD8Y!7JIl0<1qBingpTi?h>M@EZCxC(JV+!1x$kI@~p5JzR1eO!NdZhSTP+XOMT;7up_%saN+{o6M& zE{^S!o7^=pn&ipaaoQw?_4g3C)UV3M{L3>`o!+rU6rVfu!t?LYw~Nis70MhRDHZd* zJXCG}p!E7J<5pp)Q{hY5go}KMWkb4{#qdEx4=aH>C%Xr(h_tyvRFNRPzhl?#U#y9_ zYJ-cGjGD`Tes*~)i8+ImtmN%m2C{J=q)WaXEcJhrf^VS9L&@*v@4u1=IZf|rJvC#) zY@48t-klLU7Kzka*MuuDFC0<5=cKZMr}>Rq-t$*g2orqJL~h_$7i!(U@y&eaN*$wt{Q>!tok0n>nSYK;XQ!U3vC1*T7FL~8v=HKB3J1H4~&CO3Cb{=n$2t1CQaF4 zh6Bxj12oC7L@np_5cBv?$jxO{_rypU+*p!sg2szzJjhglPxifRQBGuXAKrf!#}95U z!hmBIy(&~lN3lUuJ%yc{&;eM_-wA4igri~zB^YF7D|FuK9>wGdQW$JR>M%ONa(q$i zuhe}xE?Cqg#2dPRg2rC58S7-_JsX zcZ!Ou2#p_=5#e!|igj^u`~K{6{x_K?Hl>_RAu{ z%%?xEq`lkLTJi1u;|T6CV{6p6t#{MpqYrrl5hwEU{rjh<(ib3J3i+l%AQwjsrC~!T zLelNW3`l1Ln1rSp&%XnUv+q;>LCO|BJ_f*o68IUP$FiE5o5+VD0g#IF9}yXTs9N%@ z`5Gz6b+TF=?Nq;QdirZ ziWI(v2SiOlCw>DxJInj|^3kcO8_1SIZws~Lj!OQMCr4x6L95};8dF$!SW!_CWv@cA z@)s?|F9t@>bLwlyqgP1utSF4wuwD?A@P<^{MTh5q++SxRtt0~Ey?6y*o~zL3tE=HD z@q}iHd?^C6fPSL5VryW>x5n;J0Y8TY)HcYZIhQFy&M+c)grt^eTma`^z_CSB4SY9+ zs3xI=^TPN&Oc`cCz{R@)r-K9yA^!*1Hyl{^@n=-CjF?cpY3Yb|TONR+%@^mn zI64XlJ&)ix$zJpWKu#$UZ$QEsizA5Wg9wi`JAUT<4Nph|3HwHNF;dk)eX~)-dYQ}I z1S_Pb^dQm*GXrQ5^_AeyALylJv8|ES_Thys!wq`2`W1bc^Z|XBE4C6M_D2TF(VU~Q zm^&Ej;4q&NoH z5n|~?xWEz=J0z!G;i=hq3=cTo)^!m04d82S_`wJRLBKa6Du$1OD~CvniK>@a+_10? zFQcV>iIXbX$r-JW>T~OzgwBGQ4!8tK(1c`1He8D&8t%ahOVqQ3z(IB#d9{JPZRO^s zg|&h}ZUr+#2XTba#v^@>p=jNSbn0`h0tRMKhb%=`l)|y@2 zF0x!d8=B+S5rQiK9SE3a(W@8+3tTSo zuf}h_eRMk@APoN~!Hw{lOh`M$>>EeaC&kMwb1fsAMmE`M9OmVdL z!lNH0^|$zN_;3vAeKB^w*zrk2_r@0HjeF0Jcow@!Zrj&Ts`gS*&f z=i5Zn^|_bYLWf88ep|IJrTu2nmwg}z1|v(9rD5}_y1L*g7y_Rp#A|j0B*c=!z_$F{ zZ*(Uc#HGGyC#kG{wfTv0)$~=thSGJktxImZrFylS7`+y_!OzH~#=0SyK^RCcUeI5R z9LRUVjS@u4{PQMA?*XcKRUzO#p(VwhS7f}fV@cvl$0aHy$Gw-A<@TNaVK187e{}WL z8Te3PN6oYx+cubuh7*q)sqzY(-|)n1(vQ<*wD`T+2OZsU=;*MSko^ETusbjm2KGNj zNEt9hWedi>VLl2@CoiAx?_}s5!{$$##Bp_E3S@TRo2C>syGQ07nxEqm_wO1|{T%0M zVN}keUbWI%@4&vP7(IP`Jg4H<+3cBOV2df^Dz+gvfw91jGuIVnbQWC(J zp!(?ZD=VqZi}k5d0ej(SByAD$+-A!pl&+1IbcP5qG1U&de|+Kz6SGj7xm?QgMfH9Q z+izCttr@Kj_lnoQX>Ye81)2uwNKY@H;Kxe+6W`9>N>^9yuyuUTbx za*!x>&>fDTvcWA<+d&6M$1M^E35A>SMariNZxC! z?8PI2ah#ZQW|Nbj`Q2}F!S~eb?1*%dAHOQ4Z1|kWUPGd_xB11 z)Q^)N_{CCv=Y<^{1BMxLM2-q>##1i9)X;D#&iVX7%iO#Vg{ASrG(Bg2r;LeKbCHy> z3d4U*8PnbsqHVCJV&v9RmzuxBlS_1{)A*sKz6|FcGR7p}Qh@f0nnIK?NWtB2-sANe zUMCh3tJ=@xjwCpe6GkSH3*4Q3zk#v`Gd~r=wAWk{0?CT^x>js*YHBG3k^^ueo3;<7 zt!s+SwSUZmw)6MEtl~{em#2$Xa}Fr5B--8yxIEv5DO$0n)#P8I!u=n~s`Whol){a9 zLYN^Z*plH9P_va|g?5In-L87Hfz+r)tW?~+ys99TH%U@Si-x&hipA#|D}tI)y^%3m zieH)@>=P;gTjnn$f7JEwTq}Gad{(6X09XL#x2!W(N{{L+e|&5n_R4Jo>)`(B;geXN zAxd&VNZ?72>02D)a;h6RGG%71Fsb5SGpM|Y{YXHdzJ0bp@6g}ba=wFdQfoYf9+PyE zU}ar~|2WddKOItC91cW8|68tmG^1zr>eXmyC_uu9BNc6X>b;0_V!EvYIG)|M^f}D| zx>=*DTWyV!iWp;vix~hKz$fpSk>@-S8H1H3PmEBKiKCDt^fY!MLQY~|mhhhW{&O40 z84OBG_@mr&$GOJH`&;poy>=vVbBs{@@N;J#D|N99f}e- zC!~i;212|}$iyr5zstA}F4ZW?6x;Y}v@yo%vuLTxfCKbjazv_U8IZNRj{?FxGECQP z-pohEI1C-mqU@ABiWj^hTsd@U)%CkhlDR_p2$o=Sy>sV|s@}R^lbtY!&I3pxp$9Q% z;;&rSeFpMzjkK$f{G+MBK>LfC&(A&2%u4V?pMv(c7PH&&`rgfqIP2SzaB-xS=RK;m zU;Zz?KLfdFJ}EwPn!z~vHIU!ix3|at&8hrq++S6H3(ysj z4uZn;#LhriSVG|ffY)GB&f(7EO(-*YVhzR*Iev(FfYPX%qx#oV$`G_&!o6CY(R!0_ zT+-9W@uQwOD$?ayWS|u3+XuT)c)}#U5_+2>D9}!xJ7rQjpeT~J|9CIjlp;7tgF-`< zbbd{_q9ym@A3=&oeg!GY82xjQ;+;pSw^DgbFCImA^Bu+TvA^@JzOKi7aCA8yh)zJK zl=dz&Pt5z3(ptOYI9dLTTNfdt)@uRuxz8-7q4^qi*rJ^RD>wrVy+8Ms9kmBg+?&rG z&IYz5^nY)C4R}E8M>{p)vGJn1rKOqU{l(iE!r!&hDy$0CgjB8uFJuO1PsqtJK~2Ml z8qXW8)*5LAX1ple$Oau~9Ys@hyRWa0Oa9Itaq@#?&Q?{A{F7`DJt@-7F?x{PPQ znlOpiw-X>v8RQE-lu|Gw;RD-M{Xi7^;No;FEi;!L(a16iz7rYA0|4%G9M|aceYbuu zC&YtCs{-!g#}KR&ASS!8k@IJ43vD9DmdbCu#&^`A)Fi)0#pVPVw= zS0yCe`i6)3Ab_Z=sF2mjHdbEJ^3HNy!!0=4)@%?o-qzaI=8Gyo=k)2IXHX98ZEPJi zN-X1^6~ssU)99{!l7qOur)E)j=O@m-`1K3+FiX<^-ab0~}o77yb13m*jkD5Xf= zRj;El60t`BQ@*(}Pbz#YTry1N{a2`Bwqb_ds_;6uy(vB^4a009h z9gPRVH;a4k$Hkp4bauk#9E%?SVILBA06X^yH_<#PgDf!2#B2$0Knm(U$~|l~M9`}F z{?Q{IqOSy^`VwaZQAHDc9%u*&!NkkPM;w+IZAa)8$_0!T!mn|I1i4Pg7r>{#WK(Dn z?!#p&-3&9=`L#l3k;T1aW;Aj;bg^&=%!UyM-U~B@3%gu&Vbl&^M$H$z4gl#&@Gbc2 z+v&9t3Z;q>!&t71DdbL&Svi2jSV2*d`Sd_Eh}8-@tqzE{39~T*B<>!*Da94aKB58r zgOHkz9D@9}g0xw&2-KyCId$@+G#(I%J9yP&iA1K&paghicaiI0xv`H?c@?gpHGKsI z`dXNNuOWS-CI{Opr3i-rdL8p1w(TI4QO=RlZ^puG!EHgG%+q9H%ESa*ChuR zfxZDkK{CTxmxA%OH#a%jd2=t%)kqyvCHg98p(scGqH1t2iuBNWcE2Q_2hNIRIuE26 z!LM*`knj(}Qjm5Zy;J^AT&@}w-q^ioxR^zRBIL@DnFC2bngysQ)RQOeVb6jrkeJ$7 z)N9aS5=kgA*^*Y@2oW2EAb=80-sjH-ekUwCu5q2vHC<^HU>OLZxhbTNi*09)rK<_D;pco6}1b}cUN|BECc2{8G zM-|P%izH*TU2r(muge9(=>YnX1`TD`_Jfmv%SOsKn3FgOj6>W>u9M)1B->3D2|a~; zS0E`sf6{@=lf_F_2eHV;Lv~~`Ej8jgwxytL2o7DWz$|U0;)aPGxCS`li4EYu0H#N) zk)?+mbS%d^d$g(GMY}Nc5O+|{2dm-NX7PV9;Pdwn{{QSM`0D>WbJTmj@S{l3A_|4F ow%gn(FpZ0f(tMqj;+jnnp33=nhSoq8|BE6gEiaWSal!3>0rivN9smFU literal 0 HcmV?d00001 diff --git a/joint_trajectory_controller/doc/spline_velocity.png b/joint_trajectory_controller/doc/spline_velocity.png new file mode 100644 index 0000000000000000000000000000000000000000..d427f4da8819e2ec818bac73e13398f9404006a8 GIT binary patch literal 32183 zcmcG$byQdT_BH$kL=mI~q@|TkX;AoDfC19o-6CB|w}6xg2#9otDBU0_h=_Cx($WY# zYkTg!&wa-48SneYJ6^^);~c*1?~YHbHP@VT?U2V0@8aW9;i6C|{QLLNN+=X&EeeG( zjdLD8xl30MAD~2?ZmT;zu`_jYF>o|NJuq;xx3+V#wlKWvY~uLT!p>HJTZo&F^QyU% zll@aM9v++j9KdboXvXv1V7m@Za?$>t##0oE&;a=tBS$*h0)_HozK_18?D}SH!cALw z`1rzRKX1<^0!&8S494le7u1HV?A*C;<3Cn>-xeyY=y7Bc=p8R9b8KM|>egO4e)2@{ z!Gqh_;~ZoH0R`I4L^2O<-7-iNZIE6RXJTsc4tk!{f4uuztuN*2hU9OjD|CGl``SrF zGQPp^(|Jc!y#@=7{KU8VCm)grksc%#`3kP@N!e5cEqfUad^Yg7fMy7u;FR`1F@(&7q< z$_2^mvoGV~u<7aP=gfqON9kE>7xb?g**|+$__#S=HGgJh#d5!)=gNil^sDVMQ4RyK z`~;l0n9B5!r_X8jC#uf=BIm6g!Ak#8^JFU|_;c7r9^z=)SVl!MOjpwd2j(o3`(N{e zL>U=n@UjMFg4h_7J7AnT&&RPwS9)g1p_PSrVf^w;<>X8Y<2ui~*BJi(`S_j&dp_K= z%ZAj~$DLW?*N8`HS!>Z~6?7B&+vA$ou?d6$FUicAy!bg(GBjlw68<#vMkw55S;96$ zEAt_DRxmxIq4_(cfQj|YsbqZVnhm2>2s$n}huxrA9&K!pC2!mPUa*UkRD+RG5;-#o zpPH7|xA|``I~SLJS*bBG_DtD7@8Tp@Tt;S>J^RH)TQ^gi-rhkf5{(GIdS{hFCx4ly zC`-nS(CO@J#EREdn47q`Kf*%jHiRoEV=|m`+^)!yvk%KFG;xGWN+Nefrxc$w9z?3m zS_RKSh^!-tdyYxdB*de1a1+KK@M<(mF4xdbKM_9*>h$PFS(Tgx68SKT?gV<+cecwf^ zIAXOK8QDqTe)9%HIdpJ#oZI?Jb!Mg;k<*4E%RY~W!S_wAr^t$NTReA()PJsa9j;be z?x-N*Fkmb$PZ1oPQ;R&tFj>JDFV8N;cZ-Rv<(xWKVHA;vQB_fnC`t>Ws%Z~!xC7^4 z@3FjS=j?n#>h!>fM#LUPNl7`d?8I8E_sOYCPLhdHo~2qc=5^&>npW&b)p`!#%!UlzX*N(xwvFSL{Ss9fQLpiQ|7bOO8?o! za$frpz^(I1Js%IPd|&SM7cTY8d`e};s1Q7d zjm;n?MrB}NU^>>`(UED^pBhlb#>SR;JB&PLCn%Win#G+2p-*VEu?4|5v8^|vPkD{n zFU`--EB_QpzG2hsf02~e=7%6ND=Q->=cSXQU0FFfxzb~uYI_U|3yb!<<)*#tUkMrA zZ~7chc^WS*W{x!b%XHbk{lGzhb`C24Ol+CO6srSK=vsd7CbxPCih@ViKOuqQ&!0bU zA74|@mG(Mv%8e|ypOL9OTFJj6>d1_`3is==zup!~#=+~dKF+xRduIn-s9E`W!iSMR zN&xOvd03rmhE~x0aX?gbLPF4!?1u)QUtMP%YiVh5Sp8mVxQ|aj(0+2fPkiM{@cxv~ zly#EdEn8b|$EQyt`pe698ajoj@CB(Jd_1!KUR--ThTDFtpM3Uh+VHU9KqzUoB}>hSRJHeCm~Ap>^%tw*6E-#spc;Jm!Z;9n`Os0#s5*6%#N{mxHC zr}gpC>XXbUspe15e%a472gn?2!!QLA(8}DqhXWVi+8xm|TKF6n92Irjvz<9bwcr=new~3x|;BK zYJGcp6skXc(yS3j8)K{L&2N9reRA*N!$1XAS$+MhFZ}$vJ;Q?l+I@Z7Iwy!_3vB*JZreHP?HlK2( zx=o-|wE#zSR*0+yG3ml%lU4lwwZTY$t0&a&oS{%gRbu$yfbe=OOIr<%NNQ-N$OR zy|}PoF!=8NJpc|7R6<0TFZ=7%IH&uZos#)Hfpb^aWb(nOGgewHFK!q`Nd|7sbr`h- z;v)$fbyY}+jFjymx>&dFvr_?|QM-OmBA=}LvyAv`0RaA~y4{Vdj6`FPGItKz!z5WhGz%HDmi%F=lq9{jnZBz3)o;vL4(c0h_z z4gV;NW%A?PiyIlY!!~Ebc$&MqE{2hFv6rPNuo^YX8#55v(m0G8t0o}9)i|GBXr|8d zux!+aG%_zaLu}X8*sg(Ex2LgspEL`};jC>xzhrXMXrXVk*w5h< z)!sy-->EHMv~zM2^e1v%h*RVB*t4$w_>ofY347e;@87cK=H@0YVL3kpKJ;8s)4d8y zN~T%d^S~bE+t45_=CXb%Kt^#?=W5yYHZFm7>bpdcgdCfb0aU4}siAJ#+w-nZ)C583 zO|r2vzl*-jk@wFSp$U~On}4mZM?yYvb$8EDEz~q^&=u0|c|UB)QEr7EmQ!@Oj|a(i z@k@H`HJHmBU%3aagme<-+Rqudxr_K8JunvN>0NQ$#b#v8$WXCt9;@?kDL-S)n7zng z|0pwnh*vznd;9MnHEd`nB|Y}un)bf_`J<8pHC^5>)k2}?`Mk05rsv+MnZ1k42i1|_ zyvs$o005NvRBMKQh;P*T6>y&LZ0h-(kVQvF|D0-&hCJ!t*?G_ESy+x7UqPAt6n^7+9NM?$XcfEDynV8!A8DR%Wq(09lK184`^O zdt7H%7Y1r|s)3%TZifW*77}rLk{+Xn^4HrI+D`xgXcY)H`PgMofAhI6DC)kt16i8^c6@g{S4$$FQHe=6d4YNafiBiY69N98TWtz=+C|F5TUi-8$v9Ykg2Fgt2FVTn)Ier{}J;7Ke@{}TL z_!&uX%2zzOtQQnn#Kgq+J0Vh}`J9HFo*XklfWW}O089@_GzhW;0zg9g4IxE9 zjYD!!&;z99c@2~4z zi<9Hn3BD&LgEKy4&Tvj)#R+2wo$w49pm?)N|NRiQ)JBLYfy4P~nMNIvC|`P?Kg5}L zquOI1Nm)QRQ&CqG_@r$b4h580foe;QaTkf8#o)R0^mKbi#}~1&q_D5T-#F|hR&Rf`O#F>Xq=GY)UhGN3rmj}Qc>!x#SwWeLqQviATxdF_!vLWRLgOFc6e@4)wKAi{ja@ zZ*3o+E`_g&JiQZ_t{-#U^Lg%{Y{>vOHa9z4@XV5t^mZKA6?+7U=eUg*aSvxaJ-GZM;%WHlu(?#42RIgDCR7+5P=#A?TL!2^#4b zIW%|yeOR&}6$PV5qklr4BvPjkD>@`@DpjfsT66#X3nu&~WSH1Bd{#3*svXY>Cm==+ zt{!~FfqK&mdm$r(ZzhX9tD?O6{6D|Pz}53fWUi{=&Cf5iD6(-pdtudSBZ|~BXcnmY z;#OuT$FtSm+;sf(wWq2pC3C`>*V(!OEvyi z%o;|ITe-?8{TIlI2BEk!*|RsypHlraPtKdtG;A&y95!*mERN|iF2P-RrWiVbOYEv{ zW;S^2Md({hq|?SwpAl=sJYcTvPT=XNb^HA?K0XjI3;OO|CQi~!NIJ<-KqYT&CqB8Z zD@B6O5ej)a($>} z0X00TeB?^OtsShU=S9YJH}V_`p2lcnvJP3Yi+w3tIVZ3~zT_w}!RJ^gz;_|BY>y02 z{~VG(axJ#2i~Rj4?SlXE-uPGTH$F!aFRIRwCGW()swKD8t2f>=cvFc?@JfLuTdkX$ zo5-`3%kLGE49g@XBxpJ!X$5Nf0d5h~(b<-9F*0K2s}%>{$$T!8UUE+_Ky@0P0~3>o zsMF&>kbio7%;&99gQjUW^*cG;Fr}~IT#fVb$lm^7nX)-q?SKVQTvu0@b*!$o_GN0S zt#jJWj#K&C@87>2=GyUHJv=CWun7xOeEj&aeMv*(=-rddFE=y4yvi$1x|q~?qU94V zfxYQ&9GRpl+PLbKC8n!UQLyv^pNanZ-Yl<|5=T#cd|DchTiT^dTO2ktHe_>~D5LZa zk)QX8FL*>T5Uc$5ADNyRy~}_u6^L2+0%~BoUk2T0Ps1zCzQRz&PcdmT~<1aOne~-q|vF8CVQM6?4w~KZu zV$ZH7bmR3dni?Ble%&UNqgI?cx)&K4xw*N?ThkB44KXF9g=gwN><^GbEyKf=%3i%r zeXXztP$)V4%|n^rU3Sy^&+XSVclc}yYvJjy-nc>Tb+l8eE_(l2qliSK`cFOrccs>* z%|aUD(P9`^_Bc6ze}CjKzYBynY@;F~+8Uo@^{$grQH4S&F=;%8#I1DMAVz5g6?QUZ zc9rxs+<=U)^o?|qlE})lwd6LIIX~PoRnmo0ktL(r+pAb1G5Mwxul|GozaI69G^1mQ z^96Qxetr@???aaQqZJNjHa4T#){uMm@ADPqi$7aYMnYoy9+Gze!~_6xfg|Sfi|d2ARY5p0FmQFVDU|^Vr@DJ314Lu(eF+Lbhx)i^?xbUDTET$5H{gP}M%*4F;)g^@JV`~eP#KWY}%Y88R$A><`@5BE#G?C{?- zVxAWZJ#n0DC67hP`3n~=beUe%*3nrzN=U+(d)x8vc_oYROHNMquVctq3G`WkI87Y= z0I)uU0Q8NBi^Xv{%IrOFM2A+7>i7Kri&AWV9{jE`>>KPfphq17A%XngdDF_55Sv=2 zrsy9&e28=LV$)1ZP?$~H%a^_{UlNIkil)OZRR8E`!m|5y`!h+{!GUY1ahXiil=pEV zi3ew$Y!HD#qb~+B-p2ft^~dWqB>Sape2U*hzOT3@io0;rxy=(-S64R<4Uqzu&>VnA zAvfOttsU()tDkT5BA7GmVlg%XCrMcq8Y?V@UM406mz41C{2CzyL}farSMNy)Y{y-7 z^(!Q#q+6378wP;A0NCcvr^*)>l=4>F&tMJ?4u(eos#Z~o;$jjPr?wfZYAZA8mK&Z_ z?LceH>FZm~F)%P-*wr7hK71qc{QGzH`K2W$h-9Oqqqo4(TgGiXK9kRv%h%W*rE+Rkm>ZO>0nI@Hd%8@Wo6u$osoh9meT!j*)yxG8y9n3{ZzgyRhGtD=W!L1UcYmcN2gE_p) z%}oGgJZDuQT&=9NiAIZx6m3kY;L7q|nVpz~mH;$Q^nZv?oG6sL5C+`)AfvOOwYH(VPd7M9N2=S8f z`t|$B(bq2HmO{N`Rfpa6N{Ae)ACj>u8`6fe(;6Uu-X!vygrcqj!BVhG&g`(%M;o1( zcnx+2HmVeOM&y^%qhCA>{wLufW$jI>9WB&$wG89q<7iLM+I#Zy0#c5Sj^Tfo3L8O{ z7;A86S>mQY5xiZycAd{ z!`|0I?Kfb8Ka*b2qJ_!O;Ac2>8ODO;P8^MIh|h!k&!`|G1wf@R1SB=`hyRLo|Gyv5 z`{f61PNb6#JCz_0RhU)@+9d$(bNKI|TkybYIi~dx-7Y-p|kWBi^GLPX<$qs|{ znUD4_MW-i&us<;1x;}&lJcM?12|~qRAd?NYTom zy7xt!?l0k7J=erN#aK3^kHa9sbQ>ys4Ba9*V-vfehXJwN0|$Sm#4XSDj)x(HqqD7) z6k`#AVK%J*FbCw#-on)>HjD6pUmu^2*^ z9s5p6R1^UdQbn}G$d+A~%PN!U#^s-3Df>sy!8tJ&FLT`Kvc;;rxluI2yENch@uZ-m z!R0IK@nr>HJk|yjseVS@O`PX|BggB##(wB9l!+1$5I}hzq?F4!atcBXd(uO;&DKGt zdve_`3`|G*Wh1tF%Qn&91w}qDBogm)zTZDYJ!zf{AZirgh&n5-(^V{Quw)_CW3=L9 zu1QTzg+2R6gE=O6CQuK^O7pKdiXXkJV8N;|raXK*(tJ+4KH6rS93IOilfErgQX1u2 z;_jzv{#JFx2kjE^E7XIkaCDx+^(E@*VnU}_g=Qh&=7PTn=)f zy!n0fg26SD0n18w)I4^EbGD{Sl%!8UND?6$r6nLRd2|zo$yW-auWN{0{5M|7dO{BW zrdenwKl@R6n}0%Qvx1fLu5;v?W|8p-O_IWSvpAztfd-VB5Ac274z#+Ac>G^i$rU8T z4?f8jzLNLmZs{r3TsBwmjm==R_jW#qS%){3@m+mi+|TNiL~*&xnBQ}M!lkW>lF#tm zd3-A66(ik`j@bN3m(2R!z7&?AFuGF4#r}c?EBmL;JMD-z;DsRBa;mKE8rIAoc0ToD%BxAr!s>nS0Tm+CKN2C0G zWz8+s#+dVr7b#T@uO;3D`BYPARH)-6ih|RNSMBt1$?D9^U{Icjhpe)W4xD=_P0B82 z74s%w>^^BPx{(jM#jm_~ety9ML7`>7D^{__+2(7$8X+(v{1#72-ZcR@LlM!?TuM?1 zq?2?pdkcgL4x}-jO3R#$?JfCB zGgQR=7O1kF5EBs>pX;FaY0Xaa4h5>ewY~kd&GFD9X$;if##9uruGSNuhOKK>SFO=^ z?#wQw`LsXBx-dOG?c3ycK{k6RF4yk&Vy`U7b@oR)778h12(2%;_W@F*!=o7oS^CI>UvpgW%KKkM@bTTb?!nEl9JJ{Uiky1 z9QVmi=aW;@tLsltsNQ7ZjxU*aOU=J90w;Uncz>f~wA{?$aN7g|JqlHSdSnH{<_t0t zOi>bsdx;=@s#jT4g1`h2^f`@FQ#vH}W$FNR17aRdyzsh^%mshm1KbDvi%BNNi z@zgRp?h5-8w_n~EMThdrJ_YLlWTQx$Yp~z3|V5kK5M&gmSA9=Zv@Rm&vOfVWu%xe zxxo2N5FQ`p_cN*M!|tbVk~dR>c>Uqo+kJu z-cuaJZjJ(SWqg&JiuBa{Hy1Kx3QIKOMhlg$yVi_kjMuu6f+*>FcCwEE?+WWt5>PhT zM-HKeQ2glz$PD$fUr#Vu6y>7xGTbQ3%gb*k5%292oWxhScloZC}`qvsjUrRuuT zWS_ckKDSvj=_n|yC~vsk`)8#Bw@2!a6iOe#03%EAl;H=I2n5iu3&% zP`zA}mYvN89D$kV<$~Uj$jCcsQeM2kCsde!!35lY8)9*|&`mT;SJ%*M<{A};Q9dj7 z{yf58Mq>=>TU@5?UjBe;qVut(ME<1njiyX`eIlE@sJM_T{*D{iu3N)5P0DnJ9|yeU zx}{=CJNbf!mbMfaaL?1jB_wQ~C!kT!iSYBox?wvW43|R`%0v+dMlc_!75%O|*&9Ph zF%YEr%=&0JLH!4?ilXC8SOhT%>YQ}YxXj0^`C!W_r|(}OBMabAD|&lBmX(#%T)D0j zO{OD%jfaQOvXUxU*sguL$uDB8BZ@u>!pl_#hV%ict*xz}qvdQMMcSX8dYwCee)eR4 z>Mn@25jMam`UBgH6by!a)-GH78v|_lMcO3yVIiIcxQL+ z?bG(n?M{liN?U81JS&w$idZV8Rt63ZJeb26R@bJ9L&Q6}_7#pKT^yL5T9mrHG*8P`v0EJp! zUN#Y9MfIeJQ-JKqUEu}T84#C=mlbQM=Gefb@~#LD#{9Cco7qCOx87NI_H)f?bTh_1 zH+)rR6#Af*EEakj0sdJc;FSBZ)a57g1gENwenD{IgxTG!t& zYQC9AsmoYqr*dPa@UMZuEj?)@JUm=RprY+x{sHQ_-|o9BafgYpXV~NN2Ao0UoY6xo z(X@~&4B9AKQ^JdqU=)uqL3Ozq>kRC_jyj$#i88_$eOixo`-jgrp zK}U+M!b}jJW>f9B5L#Jtb67pD&iuh&55c>E5%qs6;itJ#v41@Tc5}Z61_oq0#y{BU zB}LxfyM=Rt&$8+;TKZYlUMO(C<-LItHJ9eMr+oFPui^uA@krkesdUK`fJ_mZUnWqC zLg)fu$ADraz`@6F%TvjJ858pY5+Bw;0@b_s?+t+-N9}O%9vmJrT)Tz?Y{dZL3xQDJ_S_y?c$H>l(i(vQR zJTh1s_;Q8HKnjR2=1S%DAyJ&0Zg z;AEDn*4TJmwQ29@nY&5Su#sxI4WQdTxw~)Ne1f;m(mhC)#MA)N$Q)>=(ML{bviCz5CF6-FGJZ>xGK$vLyo%c6z2jaI&=b z@lf|WAROfkqgAK?w?HTj-l#t&$j!}loe7}eg>eI{qd)NWmWb=-Tb76N%BF#FS8%`m z+E#5+Nl;*IL7Z)R9h8lJ?}R}`RoQtTjdnI`4JL-P^Rlu=aRFI0Vq!mjOq6Jm>%^}D zwrmck6yWta^#l#y5d1%zJ3FnQd?S-6pZYpS8GL}O9DMeprFs32hqQQ?@n3(Wh~hB_ z!BW%!I@6Gz=}!2y5eHD_w0}}1hnB>hJ3{amXs4nyU+wU(Ds6X$uPrf6Y^o-#%4U*40f_>DT*UtCKTqnZMVRdOvmZ2Ebc{ga_s&68Iu& ztt#tV5IG;j1V_?}&qm*+i$3~_&N3`A6&@Ii2$~z2dkw);JAbJ*Jn2|mCYF9kpb*<4LMP)Ganxx?B^HY2>?h! z&Z(}yebmB&51SPkx$}f{mntgc?XOQz z19kj%76zRTJdp@WTuO0y40lj&0}MVw3-9KJ$Qi3Uug<)lfbipJ`hg0Hb$eA&Lnt)b zZ60w-How-*bAIoV7MVk*$a>$dNu226hSUW6i91xve755vfW4VOTHD{4x(jL2d3`+O z=~CZJ<%n)ei9yRIaGvCD;ox1MxB6Dl1a?CE(?4zsZ$yaC4*PvxzI=&7A-*ZtyVLXY z{-6QPg3^T8YF+{|0}wJZYL<2z)<%RtKk!<2K>Ik_Tf=j4aryJ-cmMZVHvyzL`!MO= zJwG1!h8bie@Eo*q6xY6Z_l_Akr2Ph%EpG!28Nx!dQTv+!A$K70SRJkSGcJ@aK_LFwe||Xad21F zi;?MYT5)oZz11ebcD<90*Fi$Squ_3Z3Y3@%-1Z2Ji=+|#E9y?vxO71Ad>(e9^>{V7 zLNHN?t!a+W@@vlX@o{aa+xx3+fWoPE*`WJYtVd{Ob|Nf+)QX4^r;9DEvAY`&HNCnT z&eQORL@sNv+769A2VO~|Kkwb3IR80WCr%}7t8L9`+V>^|+=^HFr%DWDt;eeH!I;Mh zR)k>PI(Gwra_y&VEG!0-wQg;!gAl$NfA>ohq7Hs$MwK`%EB%2?0VW$Buw&t)AQ%cR zDnoEXdwWX-UZOOty_oQk(u!SSn{v|M|6KVqepqB3p!;M=(JF=jz3sl>z%6dtaw%U} zLVKv&q&w$R(xjKzD@PU>yT_m7kl}j^d7ZT%%o9?7Hf0gyPe(@wTnOvl+L)Q+$k22}NA{DNX)i*&Y7>XzeWSFSFH==$Z@EDb8QNgx>Z~)X7wl7$kZe|aGuN1G{rcmp{ zjr%Y`K&pK&F~C}%Y9Nfb<9Bwt-+(M&If-lrgA2YtDy^6Rrok!NPZ8V9JFf(8olPi? zZ{~A->d6B+zTHP$T)d^N4I3QGx2x`b1v0^mKB;)NKGkO^Uj0TC2zkgn1J(nv8?gm^9uIo4#6OZY_=-~E&3v~pzfn(5 zboW9qsJW#^9Y{9;r_QI#L+>B7L`k3G`1qVT)?ytqXf%%%>4+QMIs{Xs>d#%M7W9_~ zvy@UIbCjLL49ZAay7oO8M*6zYFGJJ2+e;?kSRzBcn(r{yV1HEmf) z#Pfg`$=!$%2DNi>4eUeDU}_!nz*B+wtWKaZ3<0w@SI2<4^nJEO-5Z(|HSqFO)A# z$qWRwyHM4HpNKln2j4#i-Y(tk_u|X$ZkMKX1Jk3KeQq#&Wyw>(v||gH_V^YT^FWjj z?Ck7xOzF_Q7r&_snxPDipzZh->64wI@`X?xcAC}`7*}F|qj3GEi3wT=2xxbA_vnko(9{7& zF{A>M$sS4v5diV%!hAd~H+^u4UW&%9ZkHVVqM=CSgSs&wIhh(!9U%uJk{A$>-)mfW zxV5WMgoK2UR%hBM^jaaJ@qwAK=d}HEYbpcRyb{90oT{6Ho zi)7HgH0dbd0k4Ai2}5$Q)j{t7K%8;z5#hbPF1u@~+_vngIs8{lpWzj?>Q~>4=qftl zeI){5TQ-Pr58S@fjWs_NRo;|z*#Q^A z>6ivI2BQ2z=4#85Pk0RW=?c3kx{bPhI#eT&3l|9KS`mjMgiP2S?XSPXLSTS+ zPb=os2+e*zJ>Q15mDC1^f?f-18e$hpn!Zl1ZU z1l{JEjruSMa7cH7hK9!G?rt04;elek2FJ7x-A?~=RvO?X!-9eHlS$_-Vo-YpVpv&5 zcKIieYhX$l1O!Nnii%h^^uZO=1P=$+pEG{4l`*Q^IbgPXc=Pd;qh9Mo23>d;89;RO zECFv)u~_Wuj-vu%OSvxfpNg6i)J3r3m34~dg(2|D@gohOt%%ARC1!?m2pb3#ZC$I9 zFR1c6tD~fl?(<@Vzm`xy=^bkv=mOJl@6n^6>}<2r4`;Dm#%;QV_mqicaJa?q1YA+I zcddT^LF^9$c%MOQn1gNC0iK?%qhG~ZHR88FcXl?y*(1JwRj&QB-TiHyK!xOfJPSlI zB%Per72(JPkZD)-G(>t|&jf7=R_z=Qm}04+ z(xN6Mee%(R2?D?`z=9tW60h$bQ}{=%wIDTIb*=ULr(TB+}dXWqaKi#R$W zMq;MU?Kdmvr?@sQ&z`jbOZrZVc9h{Szlg_DQ!tuU1`f@i)5mYUqlD_9?MHsw|AWQQ zd!W)DrLQ54Es)GPcb7Z+a2PgsKF{I2{`lc>S@&xlc03^SLGyC(cJ?o=ZL;T>T^;v~ z7a<}ZCFcY!ZGx3r;=XH%tOqa~w*sG!kZDLZMZO$tAavd@c4NQ2$U)&=&TOb= z)oMuAU6CPqw8}RqSvH$4&`*PZ{oA_SM`hD9mPf7_(-FdxjX<p(>MFvaQ7j+B$El@S(#mk13s2NdTl!6>=D!>vxU@!4yR}VTmS&Gt*)$5T|?Rn z^cuXgY|rdvG<4+?xZ40?Y(m&oK2UE<60{&8pb@!x=)jSG7BjMGYG!5xn_IoXTM~3& z)J^V$zY$?DOFmsSKTC>bG$$y57E8TcJ`R*GrhzfC`xY7n%Ph%zK>ObUnHSO^f){~A zq(|DN0N55I_kiF+_NBP)h41xVHEsP*AXj`X(0H;vr>c6!k~R|&9*zbfqyFsF10+l8 z4|Y@SfTW_qg#(3_7A)2X5}7L-f!Gr2MhQYS%9O_ZCt`KAWPGD#P$|uIMl&7E$37Z> zMR#y?lmQtFA*$$n{x}%bfZ-+su(G|gb0D~!+rugSYFsv`^n`nq*T+ASfTlK~PJA^PNhqPP^tvdi_+HU|Y1rNbbC1{R<6G zXXH5dTG+U&A^4bojug{F#fNBW;3}UBEWYq$g6#O3jrajoYTC?@?Fz1q8ai!fR_KwqTWpz<)1# zsGdmBTqsm!JGxCzQope-D78L0qW&=8#YCfstVp~#pcXm8I8FV!nE)|42 zl$^gld$rBX`99=(HYk5up|d7@%tOz1;49k6z9SWpF z)_1S^lRYhqr97QhSCRqsDrcfpqgTmU(+H2k9*Fn+|~yAvK80mn2RYPOrFL9pH%I0lTH zP>)mh|5{u$b?gDE7FIqx_$EVF?JO#7Co}=UcCT7FI5=SXey?`ObQlA625H}Fg*Jko zJ-fx8qzt&*_9dtRDC+C$8NtX3#s?^uzk&X# zw2=ve8@`>n`8A|2hA`9utPaY4V^R{ly3}AXGm)AE$jfIpDFNXgVU!_T0;wc0<_u&M zVu?lyPbkolU;yPdSX_9kG-{l=biEH3WR53mT>8K3l@$Q}b~T}kukP3>c1h7%vy)9i0w!oUa|LV*s1we~yYycEK&b$Y!5}S7k8nNE zYWpT-cgh0dj|);QExs=&gZ!_D{exc>B~27!jVEmRFtx~Ztr!M6e3H5S{^28 z3q@S`+1}pX`L_*^OQ3_+bZj4rcbFq2odG)(299AvkR%sAomzke({(OF7^y&afD3yh zx%=wA4{GQDI!37>r9i(X_pa!R7cZ_RNFUIyt*zN`hKYC`L1!04iH}`Uus*^5eC;=c z(Jpx^avt4qq{suc;OB5Yh5PrajwO$%XT5Udp?m^8Vl-l6v`j5Gj>`jkn?YB=R|HH@aT@uQNlClx1IbOq79I(}r`qmRvK<$Lc1VDG$!-ymP zVwC>&@CU&DqVE-01!{Wj`8*s#k9tOyoe&3Yl7N{49T2eMkTnDN>y`#nKTKBz=~2kM zIylbNzjVnaEbKWHkq^_PB7iSNY7w~RO$fsd&>u2bi%kk{{#GcLc%cMD5)>k#h6oK} zA8Syxok)LMc6!;p0NUfLuCrp0_WJeN`2COooM_3 zn)`~xEW#CyZXArfyaTRV!_aeyh{D)-VkEAT~yGEJXy}F z2=B}P(s1X>OaejPJydu+( zh!21s#0W>80s_DB=T#ClJUYLm>6CUel^vyNqB;@Z@6# z$UFyT84v8e{H%=#d5jnw5L%$leYY8^&?p#m!?V+q1@8?&pVc1w+mk>I;-3DT8Bg`*y4hZN5&@}fG>6biQpa8#6&#D77 z<%EINhvG-Q-BdkpF^Bfo;hZW3oU4y6tV)eFC9W=ZE0t~%^IW5~HRw;JFBq5oRob!j z_*EWV*jBIa;r{%+-Y-!z^E=`{#a!qot+E4N={*D`Tsc0pm4S`zZgcAQNEq{3*u`zo zE@uzs9*{&4oelQCD^M;d6x3QRh}{BcXnhdLets*YhI`@|op=J=h|L?ZXSe{J0wFXZ zx`^Y%gInNEfI|K+9l{_Rh`j#9eBdnx@Tewlw!8olMgz7ZX1db|V79n+8yDo0@SdI? z!(%9ww$^JlW&1u&i$?1)aD4>Vm-i@Mz*@o@X z4oo2*BrRZbNWlt$1_T34zbe9uA;}TA9uPmyLk^ESJOGa<;^;p;J+^}v24ujPLQ`S% z{bb-mugrBs0p$7f99E4X!~}>13*L_iwaQKLr4FXCYQek#@<;$U=fL*OAvubTO1hRC zQ*-%}-qTmE{7F5lJ9VP}e*3}dcfQz+6+-DhM9{q=1YO=A4EKSO#{yz51kBQzu(xkl z0WD%aRWAw4G7!*vqK%R5-_~$gRoAwy;$mPT!1$H{$tu87-x|E9yyk5enX{+(Y`YV( z7*&L*pr5L-qG+h#yZEotkuCj)&q4!;Fh$)oFKIeps@bl@=D|A~O1u=ZaTA1ZJK#`PKN`sOd2LAI``46i; zg~Iz8pgwT;^^NQP`kpE!`;h=^*CjiP(1GqeNTQHg0!*KRz=_*?BxMZ&*>!hV4O~8p zhOs&~(+Z@wJDJ*wE@ms`0(pyTqjv2w+39WOHoiPDhI4>_h%j9Zmlb2hvxl9#+IOz< z{F%LhRZiF&IG(<_g1^^F*DhG}6x2-~j`^o3b`{O>eCqqKLz9XaxF$kJbwdcL5Z&)`56Eki4z-uK~T|%#i5@CXg zl#iQ2W^i!%9G#xyx#t3VjG(m}mE}ig&1OlN+0UljvVVy&^y#6ja@4ihVd~&KybBjj zQbsR{d6qzn?hGygbkhaQUo2M5Zvz$;STii7)&)TU6NUKWpjhG_Dh1!i7H|>BI+k&C zOufI8Be|M5f=3x#+_Wr8iA^j*d7;-MEBf4u?5N+FHhbSUJJ23tt?!%Fnsk2`6> zegsxaM6rf4G7wS`>M9pkuknLPq)`#x0pj|*S9lBj7gQez>7~Y9^J@@V0klS0cXu&h zzrcJN4rq(Axbl?!~Qj z2i_}mFLkp>uYnFR`+!|b!Sumcg~gNE0MkX#>(3jthw~k4HXRFL9Wk!PK!;7vfJ!Jc zzHA243CmjkNn0&pH9&#Y58AIUFxF6mn%5nV4KF7G3H6WYRmL(*K#x!gIH4ScsQ$L`VS*32ghc0et>sOY5 z$$n_pltvHY$)C|uT9|D>YJl42g*eb8@T!Ka`toAY7`9qryUA8SqXa;vNPyA!@2Q~0 z{=OGg9(obF$fr5U6qgh8mlw)Rs_t_qB-Hf14Ca*G_IH734OWXANJ;!>>g#3aIf+Jb@lGP2kAPyDOcU9JM&XHdHlzsydBxA`wEEsR{-Q|vqbQkC@IwvSdF5b)fdfSRMnH~n z@21~3g#P~=OWvRuA&Mxz%RsXvWTLfgOcGs<`_ z@!zLJg0J^jXl{F5Lt_<0+4%039%+$M+fMbS!JQ2nHCtT>%^SiN$&^H(wp14n=y#2wL zHaGv#mQ}hNJ63P{7G_VH6(i#la*g=#Jmd`o%5|5uBMdZ0EM9i`YDm5}bndrEd^+#t zWJw+ME(hmL8m)oj5H6N1=AVNYCI};UAEuseZIYOw(R+acwKqD)Z(G1v=N(nZSZPN9 z+k52$wW`?wF4tGNla4)jv5v!|%m#tvzfB|5XVqMmiwW>X1mohw%^yUv8CshZ^o_{S zg=t8dk{$l|+rGhXw2jwT4NZX3XDI042!=z0Gc2FEDN;py@fK`89`E649Mxkid!*o+ z|1sm+*M&9?^j_C^Pq9H9IdsZI*BLfzMUZ`X&rPbsh^at2z1?@Z{o6k{Pn@-O_|nK% zmW*xoja42q?~t5NMsOtZ=8qU93o{>$eR=dSE$O6XbIE(#f4x?N86Ujw=&JCGc4Qqw z&q6w>z{5>EO2S&Jav3>Oo^QSsQ8br+vsT+>c$W;P-c`n5b9sblWm>+OO3ZY!PK8eY z*_W6}eMy@?mobvGRabkN4&+}1=TRZ29S-oT6n`A^KMmdgtGzQ1r#kQd|IuPEBtn+R zQfM0O15UAiJm4Slc-bw>82qa>^bOyzXu<)g56$*2o3N+2ZMciKOX`K#V^!l(gBfR4 zA01^(_uL0l%F(G}2mUCWDGXt}_ZG9@Egc**2U;%R6fEaGfU?hgl})h?{tv%MuvCAS z5sAyoddk;>tbynh)NhT9j6^|cIcjcDGHx#VYUvLR3zG-`gL<0KIEAAm=8}x?(*;>O zq0cD%7o5IGkklJgWs53HC9D5%-EVR1U2_>rm(ExRpp>87>TX?+?N(Cw8gm{C*7)9; zFxXo8;*2wACH~?ec0zJM-d%c7*nroicnP2Xd?e&=HFmp|iYXLuX;`u9GJe|KS8H5b z8WIGHUitMrw+Bh2(f+~#pmruXB9|;_xXwh-Ks}Xh!-IZ|+o&wWB_@WU@_XmyIYiJv z&{U&DXj*^iubs>={gOu_8xDx>RXNJu)jc-Fu_b+4&&<24wmf|d=hPwJfy>F-j?ieJ zU7LU@ z`IgBY9UThn_~hg$)c4x|04^uI5Hx}n>9gzEHD>oTA9uZ*y}veYNm!%iP&L<^e$RuU=KUv0?Pul>>&_cr8q{v(($w?E#?feGTguF_TkX*``dqUu zp@TOOuc98ufMt#G(p<~?XvrT$?DaMU+JN8LE`6Y z*LKA*fBG9ebLJj>4q4zjGku>lt9f6_jPYllF;u+91kGbdLbR%@sx}l0*pA~63%0EG z3v3d)=hfHp8T7P_cO8W;#wTYUEIgQx@KQr=vC4VY1U#(ARg+m#Iu2#3p|gt4)jP1m zuO0t%g0%9kt{ES9uU#t+k(IEltke5T)fy>3`1!OhVGLFF{}JZdUgj3Ktvhx^yF9Ph zK0DS!KvQZG1pc+>SIZ?2H5TPcg!4!U=vHxQZR}kBa7o_+X}8#4BD@uEDsRSS>ZTXl zYQIFGVB_CNnYj^Q{P3z7vJW(b?iS5{e}aMB;cNPkIcoH_wr+gRVb!kyNr%JdqLN2@YAWWv zAFI>6ne9kOuZfatT@ZV_ovKZF9w2pgkKv{j!_f-I)n_WGnkJ^8CyCY;JfTJ6w^>wVoyZ=Hg+iJZzQ<&V+#5w!J9Yzp~wu68S!U5D^w z7M~8=#6gF4Et$Mzmp`j{X3wTzRv@YllI`>L0(H8#yfJ>Bg8o991up7V!KDCImZ+tE|fq}ke)qYE-yH()bZAy z`in2ZT&4T|TtP$`v9vNwcZW06pqy*Pr5*`M5=m9hh(aw)XpM0(L>N%Ix z^U(|pH&a&YBQKI_e)ZEIcXSAI3rg%C`qa5xIiVH%u9}~my|k!X%%Qy4aP;~}g-f+x zSi=^e6at1~TIGm0)9lz^+}OAO8)wc5)9xF6_oW05L8e5R-5{Exc1oXKY7`U})`Ci) zZeNsZ^MM&)c5ERC0gGg(Oqs$$x(5e|DAZ&|D61j_K?~UNtzMB9`LoB{PlrK!(?P`- z`7Xu__VjH{pz;6=Q`>&N_~&D8Pt{% zdsUTG9lFe;%~_~@8LzyChQ`;C%c1l>`APHT`ngeu%gW=}^4OyxpR zpX#697ZvbFeKS;lba46mZd(t$)ePrV?F5m8oQ$Wftqs-Lip{SYakW*_5A!pDyGXi& z8v}J5RMS`n3marX$V8DZxk*J+^NJ-KjYjfs;5--aypWlwBK{7pTHGM}Z?7+aZTKeVZl8JMS?hb{g5LO zV}m1iV?&$42TdtzE3>GBik}WMNE3#RJ^#7=Q8POiGEL?)UoHBGntT+x%#nx_co_=H z%2gYDmrVgsRRlqr!h!`>3}HM7Q}LTNv{9!RynJ$EmY1}5fDVaH${#&4udJ?)LP4Z` zrix1SOIj*_!I6YfgQxt(i*>-OQ9za8wv$)}H+S{;Jk`_FbHeHq$h4jf8A~N}*Qz(F zlh!1y4~FXbEZW=KbUO{V83&hQ;U5Ug7Qa$OqkWgQQ zL$L{0yZWyhubD4LsPjfoPr6;9d*U+}I#YAZ;+0CMZ#g#dX3Qkuy0!gZ+KqGUiwxmW z#Ok<=xMK4FL65O{M+@C>a5B+V#$P{sU(wE_0)hh~(U8%L*?0vwvH810&_1$GxldNf zf9|o}?VgUZBtoc}FaSfyQre)wvr5E(7QokKkoxH{is)XuK*o#c+V9HOgZ4`;#L8Xy|p_ZFJr`>7_Pgc(c>GX`SbAh zyHCZpYJOH8D|(?NAwi8Sr?~;_VCJ=q6#^&>hS?1@Q*?^g$B+6rOCcF=PhLJ{n~3#cmvY8F!zwC zIvnG{r~(_6>YZGVo^V@BVx7rpaEZSld7^?H zGp(0pT8lMaw48N+gsV$AVVM%o>r*LRqsLF0t<7J*cGpC&>tg)I#W58Xe*#s=|0pMd zC&dE!JZYdvluF(M-UARlZN`p#eczZKSsJn}+!Vkx1EdEBpo}mMZS>b6JD7-{<35v` zVs37J7f~J;i82XKYiTKrvM)8?XDBOkqkJL$JK`2V2ajXbp)JIs)&cr%nm%4|B0x{J zAO7g|>a9p6{+kr!zg_@h1FwDyH)=rQ|Ekq{Je2yuT@Zynm;S3z)BdcJ3K5&~>eJq} zqjbDj)yvy$LUhK=hZNS})$uv@hf!lO4UnQI`K(5q_u&CF67G_A1&`!Rz@Jg3L}9i9 z%M$sfPqqc-A%N?m9RG*~@1zW_`6KoIsA+&w=iQ_7%Sq#f%81Fg*WAJN68Fo#s||u& zT%ZJ8DTF?1vY~8wWdj195BUwuvOJ3Y*f5g0Z}=t_+0gTVGt1ftoZ_IIIyc zk*!x7e>(LOZCY6ls>e8be{?(+jwW;dC@0&p-0S*{@r+?8rm&m+AT47qbP`Q&h)hmi z0o4$@m3n;MQ<6&-E&BR2!y8XUT8v+6AGF#wLM>M-}dZn9R`-Q|t6vjJ11cxTewziIQ zz~l3YHly_k9`5ik*%UcMQW2{}g@%;p5Gu^BPoobLp*ZvRys&HD4F`S8M#O<6L|PtzyNdaXFyW801$ zmEe1#B9p1*aFR~IFA{pPnopL~bMR)v@dPv?85dExsRANXE&Kc)1GS?kkMiA>pQs_K zA%7*;kItlmsWqN~sIJ7I1(H2^ohJlR@QnT6BLjYF=*MSY0mTBTlW_k_2AYFN>Il7% zYGxVmF4cTxqFOJ0lYS;pPyezQOu79j?}gg0!}JvOuPBgehNfu~I`V{f$DRj!n0N0G z=^*Q~&D0&P3q!K0iheU61G360$g`22^TQo~&615=^ehY$_;YRs5h_5ctAvjABh<)t z8ArJ3;-Aky882ks@$THaHdGx&t|;*agg?8wRgUgiR}8)Q%-C)WS(``_-1-Btsi{#0ZDQq*UwvUutG)As8zVmz?ayfnzdoLHv{X-U~d|?PHJU z+&sWxv$(%DzaFqlt$qRXfc)OfnDjYMxAvNkGN0r#w3G!UcqkZP{w6)4C8ApPL0EEb zZ~|Aq^m2u#3F@tLIw063b46v^qj=pu`B|Z}uHdmek;e_n3y zLFHGYdZ&Qf&uf}^p{9P^VY$1oR~jf?^W2pqNFMtJf9Z^&2Eq&EuBN_85({F}U=(LT zQB)#X_%QTUq^Uc%I(0HiEz}c6gp>7;-S;*rQjB{C85AKzty*?7 zb<&^5@Tx#~6qb{cHg)gNZ0t{NubUxXK`Zv20^cQ0HH{agH1qCv;y8yP9Q~P6!)JlU zT8I8j4vj{wt-g;qupU$BMe!fR(CusdAHDc^pWvZ7d-l9^o8*gXp4Zg2J7d0K<+VQT ztoh$>*5wz2BDLe==d3E)JF+Kt!&QF5h`jCPdGq5{I19^0`jc(W&sO|<#AtI96SpSt z_srwbA0Q)%f!9<3xj7_=RRH6L;ly&m5CQSdnT&fJ0!rV$)v|l7#!|sP`M|;HD-l}p2WW=j$$~$5S#4+P5B7`LG;*NKTs_Ta_;=Wcvprch`ke-eC4U_W12Vi zn}aZdYu!+k#OV?@5vL0`=sv90iYmi8XGG_`lP4#=*L08h+755T1}MPngbft>KxVC; zSZ4Rpn^`|xp6pZARJmGzI21M7My4b|^Q_#Cs<);r@mHoP4e;r7?9c{GNen!h+t~N> z{eQE_wr~-3-7fk1X7RFwyaV>T7E_he&`?~_Zc-2Q@s~a7lu1H`)14HEP&JSN0f&q6 zw{1M5lLc%yvLJ)|%L&IPVjJNKKo|Y8wF+}5xPR2Gbh~BnIrth`U~@oNFc+y>dV0Fj zdv5v*Xt2*}l|dQJW&|&TZI#!=D{vkp++-d^v@)O^QsH!w`Yb z5svt*1$~M^aO3HKBXu{_$dx$5GPK3w9^E{CKFlBv$=LeD6$wJ}xlIC{qJI*om&IB)sOdQj5`a$tNw>J+G?1r&qOHVVy+s5MheUWEOB zuxH;`6y%I#Lje93Yk37ES{d3C>5I2IX)>y7hv1_2x}Qt75T|5oGdDg_Q)i^FZV*~k z0cPrAU)j^U-${NCD3wZ3YYl{lPk@PpXC9bAir+$jf>1nh#n5umG!=dnyT3=5>a9!& zbcwaIm5nYOrXS4u{@0?OW2=ole|ir;JkXVhO1K%6Vgr}3AL{h)k%)S02&y>b0%)a4 zH&|d`a!ev~Gm=bWT>D!jxbmiY1EOP}{_=&iq!qm4at{wcx?TnC+4`+){BHy*mI$cH zl2?v&yafRvEaTKXfVXGv&Qz%8mJ%`*6+73sQO58rbYI}@UgH^T9;H3*(t)X_4u zW32FOBa1u)qiqWWf)g3RK-m+Q1@<76B`x2U!gY*eJ)vYZ=MnydeJaOB6;6NsFdwy3 zNlDzs#x*_8(J07PJbLtqfM7_+@u?0Ys1{3RVf<1Be4eCK!F~GJ8ZK8}S66tiA4+dn zI~M9K&Q;4r(+)D%`h699e{FiRmP$T{POnJt?ez_c;b~{>pQ=B-Us|7zW>FfIa?T>< zC&=q?wMmR^?0zN%Rfr#t*qmQritbu+2F$n$y1KXS!G+lwzUB08Zb;S1hDzUcwQP_e z-XACr8pTggsseJL%iz_(hgXdKFWcZx%%#JTeM(v&77Q^NygV|T$7proS(31=0)zK!REvPo_WF}OoM zle~BIgY>;R6@y$-Qc_|XqUY=D3*%TD*6pl<7~HfU*%vHlsh&Eu(ofL-wpK?&kTec% zxeNSZ=}lD!{buDV=uf9SdV@nJKn5svQV5F71SuCbX@nc+X2oD(pFv2YR3tEI5_~qw zG&Y11ll^=7u3*H|4!F;rg_I)0f-Z(1Ba5tiI5}l~I091Az6Y|LN&Ak9MVP4o0GT%D z$8iE&gdKO$S7X8UFrMKQ^kJdS$4XJBt@Jp$E)D$3$$6Xv`i_$7yUIekHrtN;x6r|9(=Vf_mlJW)+2k zUHH)~)AxNQ2kD45Tm=80p^Y2n4T?O?N}@vSU@rQd_{j#FYF#W>I1C;U}IIUq>=h(~_|tjnc#*14F~i z0Q?qaZ@c2@m{g{5M}PMNvu~yH*#i@m$T|M;;~hkx&ARNauesKQYIK6b%ujS#~xRCZ#no#l&fMhQvY~RpJ zLoidYa~;fRz}NpdS@J&^`2S$w|AT@54+j2!CkDQLeQ>Z23dcZ1Ge2BJ<<-`Xkl2w# z6hSdh`4vsLLMw~*@$+jK(0$}1!oHA_ilb67_IWo$Brz3L5HzcDcaBo#3F{EBoagJI zXf(8$@)*SL@qjNaMXCxLBLL0Zz&d~gH&xYRJ5MFP5FMV{99uO*BO@-x`{#L4^hP|BzdOolk|Fp{2N8yzfp`gN!(y5x zN0||N)pwC(b1@J;YM_sJ5a<3`R3!;OOXg3Cb6_M_E7%B6)NQy~AC(qiN&yN*ynu`v zqWuAK(nsXwHaZgYrZCkuOKO|HKNML2!p+SOB{l-whBP&e(enHGRF0ROkP;|@8$#r{ zr@t!_rfz7frc#!Mk_HO}f^@tE#(wnG;fT^*PxhUmj+uqOnV|bZaV{!&$sxMv${=-F z4zCcR0lGzqGxNt;;iOlUOVSi>khr>mX||J#$XhU!fl_s@rYJgEkZ~J4GWpP@2;C^K zYM+S_A-J@9uMR%qQn)|JyH|KgANDzdd6Du5VeTfhO)!z|c!wj(A&IhE0wcYnsZj~D zkEj;IKUx7*Q!8?WrJ4mH8MTxu zF?BN%GWizJ0Z7=D*`bZ(JDYgS%82&~6-Tqu@VVXNGAFpz16Niu*cL@4Gr|}`&VaQv zof?Qh=rM~{&_qyTDiDV3tsiVq0)RTulEf0mGC)%xiwiO`lOc0F0vmGaGTY zi9%eoDdJ^ZxMAi^s_OKaL!iRO+b{d|}4K0H6L=tO|z|+hh9T=0Ick|Qd-u89W p4ZfOxh(iDWM}z;Duh2gxa5I=W;rhD07^2QFF|shcXyAPC{{R~IG1UM7 literal 0 HcmV?d00001 diff --git a/joint_trajectory_controller/doc/spline_wrong_points.png b/joint_trajectory_controller/doc/spline_wrong_points.png new file mode 100644 index 0000000000000000000000000000000000000000..f73310c031dccb9a038d3d4a67d781e38ac1cefc GIT binary patch literal 41712 zcmb@uby(D0+cr8h(%m@-0!o8Yf=DSKNDLyK(v5WI&?1dUcY|~z9a2h%bSMqdvDbLN zPrS$X?Z5V(_|;n1y5c;~YlXg6l*PrO!h%2`xG!HwsX!pe%@7FE5+*wMM3$}< zd;k%0lGboiwSDL0V&Gr`d1c^aXJzYTWp2peY~tW(ZfnEGDZt6|h{4Rs$<9%ji_7}o zKj5@=Fy)#yIBEeWxo7v{ts?}2Z-Dq8sX#p690JMGc_}5K=9;>*;O40?b%k*-@`^Y1 zsg&Wp=l+_85635oo?Gj%!Wy?^k0n4K!kV%QIJfp>m8A(88RaZ**p;=6*pzFS z?|)Qyx3?d?t<4wP@4Y$vapdaHKzM_r>0WZINV4g$*RUh1GKn++A$SR~k!wBig@adL z3dI2t_#{Tvs9jzPyc8pmxc>e`0KFOUe(lHq_aDq#EkI;5zHi%I9GRS7UOp#4)p`;F zhu<=VL2Z7vMmE0x{k1#DNKnd{5@nIM-t&UjKOn$*0S5|Y@97RFS8nt6(s~jA-ys3t z9{l-(mT?#GYjg9>UauuTmDOA=uGi&>pIq+8CO14kuZ&)AeR_%t`nBzVogFJw3=Dan z{80EKgLWy!=Mh>Sn@Z7(qogan4wT8h8nYstD}R%Xsxvt4!szS z($hb2rJqzyx^lZZV0PVceQeTu@-JRWd1u1$8@+uQ<%gVq+h5{!;$Tfa#k* z5YHqG##oI723_(x8IwKS%8s6Gq=ZEkFNGz=Q9#-QhlwFuvW`X{^OuizCo;&WSLb%M z2CqH0lV{8*i+}zkxOOk3JO>k~++QTsE?>*T?0_Xj2wxDwbwh*BTFhmwmi(!=zw$2l zb}B&EBJd!Ll^4qj)BB%8%qb2uKcuHeUxV{GJJ*Vhj+^n1n(?oDi**{YA8E+ueGi@X zxKD(8G`l)yyrc!`$=41SZaw$zLXly^>R#KzYQKC!_f;p7EDKZg?7};>-hsNkrM*s? zR;n|WmSoICxm?L=F-bbooFh6pGzWy*@ONN>qHJ&;y-{`?HJ zE2oQrM?g8jxxOCr1s!#0MTSSV2(YOwp(mumI`NIZ62L0S)ItdtQ84f_WTWZEcPov0 zks-R?7bYbdm0x8_9kheh{JpWif3WAkfDlk^MMf$UvIscnXbZg9it%NOQB`0`ppbE{ z$x^R<+)$4>O+srhUKU{~gt!8AHgxB4w;%96CFOFc%?nK~o9sHgm2qDY=UJ}h!}Hc@66^HrykL@$)k7~rsx4fIN*GFjE)<$o6okYT z|019G@Z6ujI%2_sXon>_GO(`clg?&0RlZ;nMvKrzkb)y46j}tOI8dpSsx)gYdz!}` z*YjaTpXQ`CVj&EyYu+v34~mcxKXAaVR;jSuXT!;@jGOXUx_-4r*7t_ZAKcguF^Xz;*4w=+H0rxLTaiDIW#k%6I==bu1$8>Op13OO;1Q8)F^k!I+0cp)a;asjYb#|&q zWafPZdj{|ria4&ttn$SLK~GLxQjUyBO(ID`>f;3O=doxubMXie+OyfmoAZJxjQ2;f zB{eZ8t(nLchBc@Kuf)Zzyj~pBebYh8!^R`+zu5Id3Ycm;tYB7;7hx{nzD?pXtcPa7 z!Jy04;T9LXy-MOtL591zWPT^p{G;J;c~OJc$1$39mw#oy*P~-z{iGxcUme#LUxHw% z(1GnR{A*4~PiJWcLFnA->m)6BLeMTjA}{UtGfz01PQIab{#lIHYtMAoBJB(5KsdaB z2s}N{M(mF`4e3`T(#Adnkr;|}y^-l46|$KW6W&w|&vi3H6wf^q8$V4yVBc!-VTnzy+#Ea1k-&U`&>t zQAmjLO}TE6Vm@EPU3uHBN|g{SDs!^TezA zT@$kF=}AYk)#*tQx(Eovv*4x)^v^HjMNGU)4EXC|pS&o3dJ(ZWd9_V@NfWAg8B5l{ z!SX&(;X^Fi0#R_wJ{RlcRcI{qm?&EH79 zXHlKIcQ>|2ne>?#*CX1+7qS#kw#v-$iu_a_w5_(r~;P28b;CV#HHC|Bk=ZdebLk` zEJghXG}8llabLK@_zJ}97h7qNQjH@_9`Y_=$3-;Ks#F_8$pK$XN3xuEXEE>H18D|t z!`*16q}=1)?R7FU`-AIO+qKIog0jx>q*G7vAQ^*AT5GKz$0;Kyx!?wAD+G(9=>l<~ zaJXECS-Xhc(lciKH@TGTyc9G^^Vndmp-bZp15YA~bl86J2^g+sO9EIJG1)37#JrCm ztM_-p-Uf%^^qAw|IDZad>|SS=s^9pmTs!IAK<8v%iZ@hKlQ@bfh5z}>J1XQf90gX{h>A{HJO zN1|4)OAE&O;X~}F?kDZ|;@7?@0+#GEH2Fs62KN%m9d!K}^#2qng1keoFftpF944@7 z<}$LgW6jLW2zp-&+RWGceR^WMGF@rZH#{ukBp!_a-{3E`IBao`Z zut}pVa%k1+_kUzI8_No+=6Li7C*AYto;NTm=1(h5L*xm9bUzdM&Wa(OtmPdaay{PW zMF7tpk%d&QnEy~E3qGHY)#8-vcb zdQmenGMdfT^Q7>b`;U&kMtp)z&Z$<#hNtvTtcZpE(1^+Wb)?b+ZO&|SntS~z5!f6_ z**F0WO@w3!9wQsuJ)hf)2U5yX8X6Ste}3f}^U6sieQ;?U8HM!Vo(adH7_co7Q+Mh)ftgORT^AP0|dsK}ZFri0ZE*~uWqhTM+T9$Wo zcA{XDJz}-jOJI4JmP^%?>9+`CWWTb`$BM^b43w4C@m)8+eG1u3_n`x?q$jqzl~?!t z+kNXO6bv$sxYew=BaU=O*p|k}LH?Qh)@xi-Mg|=j89C>U{Oov2&*yCoZ8$lXjF}m$ zsHiBgc#NIJmbs!{K7tx(mCBcl+1%&q7GzS^Gp55@MPKy`Dx26+Iym3<6ybs=$|ETq z|M201@BI8cbPhaOcwiuEF|Er)5sZTG9S+N@l)%mq{A|mqGFOoE+An`aE2bre-0EC$ zod)f6Rxb7~2~1)4y3=>Ie(*YX*==JM8z$J&Ec*IE^zlal=R&oFqQ~T0R;e1qfW%ETMJZa5*+XqGhg zP`WsMM@NT7m2v1q)W!a+kGOiVm3r4zaC_hLhCX{?~e)$PvK%8 z8MSi8CmY0p=;=ZFTCiRIv>j$Atstimlko%uH4w1uNHt_r;cNL@C)Ua&98n6l`#yJ) zkyHXD4nzL_$TXs^1jH-~+%t>$O7z#4zsuEy@(M`p-s`4S;pd{U(oP?F(QklYOwUKGS#)_K^XTCH#XO@+U7sX~KWU zz}ZG2b;dC0o`u4lMa%a+TDZ&mq*gCsEJy4y;d0Wvy_B%PYSjBqO6|bgJ+|N=6ylRc z)&;&6(;p&1Fj{S{8jG)8sva)lvZoK+ue#7_jpbCvgoSdGvvuq)_`4_m;hRTdEaKwy z7Z+zT@4kHb!&FKYiB9!uY-vJU-6Da%s`zkpsb(xeYzWJbvMJ0fD$nJvW8Cp)`*A(G zQ2#T5}*=Gjc7(nOb9e1ZR_}8<`Z{PQoT0b&1i0-~0R^ z?y9+9U=ySlpP}iV$D1P|tYNO}@oxy|>7y+sie8=-PFMs624<+{D>92pt|v!ZkUp8k zfqc(bs!KAgw4QtH>gE>j{%C@ewE6X=cMlFy*}+c}Q+#CP!3eM9d9^@PmVW(nC<9au=Hd1#}759y?ILZc)3E!tc(l@C{{IE z+y&A+k6xNzZH;Byuk{h;wo(ZM?v%S4N9PHwfjsBSWTW{_)SB^s+lZXhlpPygfzySOApQg~Fl#<)R z#Lu5OZU`-=%4qk1duVdm*PNfC@+tXYAt3Cu<(rdpKi2mNaG@yZI2Gp<|5aTLoFLJT zGC}tKEX4S*(fe;9zfYTSyyxrT<$@|c3-K>LMsMgrjYKC112U&* z|2oZgI>cyj%(cF;K0zd;t|(1XN=*!S*qfW1nB?S8%^HhL;N!`}jd8cp zG)vzPutd^|ka4x1L%()*juomtc%)T}L(ZvPp}APG#GcXL`^pZun`gea?&(hXx9Kd$ zpVX4|6Rk$ht;xmSi+rWNyd#f0pvkFXv&_(%<=qUnzyYZZcjhsw)nYScER>~VLRzl{ z3G$$fFlMKO?qF}aQnIMM!f{j4l72YLG|dfts$5U@`Ez7mKEAOc*h9ooxYUoUXL&5~ zib4}CHCKkV_*ioL=YKb4_g}LV2o>Z_M7=T|R?RIRM3gL$%f{vgG|f*+`b@q0=H`y= zPW)5jFUpuTl^?>4xYO+3VLH@6e@hd`T$kLMlT%WfAKuA%9DjRqxbexB-F+w}zyfOb zjG=75IqIj+1ueeVB?GcMs-1BniD|(g2-1*c)jAaYWM1}9D+!Oe2F=Qt?H5+VTB4bn zq>MSjI7KT>2FY@Ab6?hpU#t<&``q172s^RbEH)P!tCBF)V-=9S#Z9rHQVTEX0b?dZ zfQsr`Oh}e9LjG!GXyccqTFH3AVve^5%$1U3@ruv9JE3S=iNUf=XL-=@k3CPIMA?Js^^kZ#4KAz{xAsWrDJpW`T z&(%yQd)Xxu_JaK5@AV^5s8G~b3lRztOPiJyOjs1YI6vK;I~$idS%86qg(de3TOl6r{?T+B#6E2gpFE2?-ImnkApe zd46;Dv(jRM9nxN?n*SQ0V9cKommjCXdG7`?qDL?T?i_AAwM2NTUlto~I2nikr%%+A zC2s?mVpFn$3Ft5>hsTu04zYULlpB}g+JtpurO65^%Dg8XTLSd zNF(M+0hZ4*kc?zyXUAJ%5&FG%E2cWupK@iqhu$Q$HU#EPRaKS8aYF%7HO!I`5co!m zU1Am%7Ro&ig5P#FHV?l_FV-@%pWx@bFChgB%9`R_SXi*XI^6{s%6tP#jb^E_Dtt?P z!yFfwzH1X-X$Pgozyw-Sxx5rCI*Y0K=JHRcWqC{RPwy+-EY{~F(mQi?_}Ma%Ux}4O zGyioc&H8)KELJ(D#*Ge6`6xnVSk4yrFd}6;8sy3b@#b}*8Uzw4695KqX*F70y^2lB zxJ&+9zvXcIYYVHs@fK-zZmWneUxqE&ljN_@q+pR*>1GqfZ+ex)Zvy#DhZ&werG)_N z3kJN**QoVU)5b5sFNXAgP)AVSk~biVY)$3x`aSz~@5L2l9bZxkI2=O#WBCS@ zg50)?qN!qDR1iC`I1Gl8cqn)b1IJ%6LYaChITEQk#g$3qbxIZUVT=YY(5@!oX4Zlw zMhnPRzyd%dLF&MI#v_mA^d|aOm-Kx&Jj1SC#AJYE=+hH?Wo6~9A1~AS>RtAAn~#S@ z1YP!M@)SQ889T!J;Q4ZG^9O#%Pms0}scU;sA1jkAXR-MvxsVH6XO@hV@_T(K24>>= z?t(E#uR(QyX-^jA8qTHa%hmaH43yMc)gE|6KIwk{DbmgCofbqh$fUVPnW5}VAmax{ zzXU3oPREg_53D^_LLZFgy`q+qmgZSvu40rQ#2sw8Oab}CEdv#Ps%RATL-M|()LI5a zJpNknf!uE3i*r!=CP*n#XJOR12TZMU$gOhY`EeBf>XLQj14Fx(bq%k{KV^F2dV^JF^|QL|1)qwO`6mQ&Yz~ZAtUMZu zBSQ&Ch4P)x65$0-Vd%|V%?HQ{kKX!&>@>wnuhxn(&F9t)ta7Pt?(}w+GuCpyyF~Hl z>%SlVcGQeje6SK~auuG#5jb(!QMA0vh>0u;<0J^&408+ic(-$lY2GB0H9A2Y#M-r1 zrplf9`zkkTEhYLIxuo%#lp5LNoDlUy(LUTYr6VDZ zaTpqm(-~oV=3A#Z*)Df*;2R#2Z#81d5gcl8Lze%tI1Y)F{_QJ;S5iae_uyWFWV=;! z%N0Kjy7p6W;y>Z3Li6=ioun#be^Sg75yC*Rwh#Q+Xsee8q-K1gvYa$PjT7K;ZmTk=0&uYb4v=Q0T|>V;=`Xrffv>=>%s+dp@M7>1A zU@n+|lJNK_L$%Oq0jKyJK#=vtV-h6NCwETb!wdD{JKFgC?Npx0$TqEY z|2n}pknUa*kx;Hm?<1RJeMJQ(#A#8RNn`#)$#{bs$@Y`UW;;8kq*IOQ6~;bAVxVd{ z{ZuY$(wV@zDFW%F!tG@nnH{fHO{+Py3%7s9K053|nk9T9DP6(vHBFo|NQG&ps3gD_^W96fFi+erCA;+fcBA%dLr0V@ocrVQ~7zV zcK$`lXv)I;S)o>|_y;$eA4|2eSP_MoKPv!2DN#;^R8ylZH5Iu)qQ5>Oqcz|*FxJSJL47jCsoHme*VGz`#E1n_V@PiTwKneb1-&Tu2>TLm8)sH7{rNP z;74mKtc)HY4uoH9*$wHVq?>k}1=l<%LYo$QZ;Hz8R%^H8V~O9`tlk!i44gFoPesv* z+PHC^R-jF<_NSF8tIEF5_X#&Q0jP*-ZO{qhV=}2E0VAWZC5i;Fv!1oG*rknMVM(8r zjO6wUlScn|!J*uU3I#ScM2V~|Y4y9%VF-i|c--m-BwIJ_dOM^iHuLR4*yNy0N7ROp zXR$1=mR47d=6VQWh%6v*@6rONCpH3Sg0eabUDx6!J^R%dVSO>1cOE5*Hh=-S^6_1b z3=JzB*5$UHo1Awc5K#V0o$W1H|6tE5%FU5x9+Llf&z}LU4L~~%c9%g9fYBoPP(crVP8RImyq-H|_Tkf~@SU9idE+D}mtiT2`JDi|TmJo9E3=j1H6?w+)XN_@@7+yvvd27ZwPqsV(grR*L>}+tV0QIW&rzU}Zv~`8|YyF#`04 z1&%8~be0L=@1RD9UOp4hTSQuS+MPN58_lwstVs<@_e()XOgDFciYp}rL?Z|l5bM2SOc)8IKUhk^b;DNX9#ySx)ekQK81FuYh_Tg$MX ztJSDBQ%*j0={bh5`iN5qesB1Yk`h+8=#C9CXGs+mA}nm|F@R^St*wz_!HYjwT$-zC=6vLaHBzW7MIs$)s_-mpV3gHnDsG0Ukz42s~r&kLC zPj3~#)Euw&`n7KW27Z=Ox){sc)yZ}MDDx&(j869!w2bdsy{=Z;{ZIh6t!`Odqn3k@ zGh1UBl99o1vNJ;ru3P<%Z#U}8XN>suq0Q&&>ST}%nMkVcz15vf$+ws7?a3o(&?;Q? z=6Eos0%kRuKgx9Gj6J}t*{=_fhDSFztcP{qU}IwgnpvQ@W)R;usq2VSgH`;Wii*;(#}8Oo z*Cd3U_%Vmz)62Ym{bByw7p-Auy1xFg5j16z!_zriBQUaHSTLSTO3HgheEEz|3NAu} zxPK9YFWU}^YXI9PfU(D)ca!VEx3{-fl6Z`yY;A210mz%h8nzO6kGP9m&r^98L81}0 zG&gs-EV{`mC@GP*ov&ak=c_?al0R)hr@sRSy+ z?fGhSt^2)r=c`le!-M6{mF3P5X~+t=KsP|H!#79L$)zhTQZceLZoR<@T#kIQNKgzb zX35LRfhQ*?k1N^OlvPK6Nk`nesVJCEG47@K{s-xv(8pHOuhoht48ihwCAZ&veKcrj zY@E}7%f`lL2z;+pjV4EMU?5yR;gNEE_t8TodQhNABI@(4dptE+HeEh~AiK3R=g zTu{idC*L7Xk;SV1`8cEbW+`IRKX(fMnRcsZ!~ecKX4d)75=?}*fUiT-Tz-dNcmJsy@cr`Ov|a)ile8ehv~2XdxL;*%m*w9L@=nc(W$Su!wG>u zG6K;ekxNg;&aR^L6G&UT5k~)Os@(SXJK$sxv81)N6>{S9`sVt2;Kmgc*TB>=fg1p| zw!@PVMvyN|;{ykI8X)TR;q&K60GV#@^ApA}~I4y4Cv#`(S=zJ4q%YnOwVsZ?YX-3NwjL5%#P)cr$eQV5j1qFRH=cvB*EZe!aK@%K1-Ik$Oc|0tjr~Z|KuhM%VX&N4$=&0T7>z!nN)#5cyJZ10 zap1gJC>fPmO^OjoSuUnAa{>&eieACo?paUlkFWmMC7N7Q-O_C@y8ZfFa;yQCxSp%$ zfEkR%lAul_O81j|``%^xDw#UHaS#zgnqeS}VI&c@CrJFK6?)o&roTHy!fU4aVuYSj@rm?@cuWyIse1!7R|}eF(0gxUn4S-$WGy z>JFMLM}puYoF3wdxHQpsqM?z4_^~ZjaXolHm3Myrogni*@;Di(z4IHXThOJxE{Va%UMtsKnnPx&EJ)@s?A~N+!-B^( zG`*)|WLx_hlHPE~tg^!y<1y@-9d+`F!y|qLqg}iljP@w}BM$88#7C0ui~R)F(qf`k z)Co}x6R-qGgt*?0!5GGB3>S)`A(K{B^-!($KB;VKg?Z{%eT~qu4~vy-jVH>Ql5!!L z#-9!o=Y9*JIp1A|!iRX4Y8*Eb&33otiZ1%d#WDNIN?L{0?XrJoz6#?OpTZrq znj@AcWnbIEmv6WV4ich3Jf42LW&Tbk&fB+kMAS1M-3Fos-bnGK{ywcAs?DW&CmDp* zaG6~nBy^6J2&X|p(G@W+#7TY{wMV6q(lrU(5R0|av*&VI-Mv8g1Lb5^%q08xg|~b_ ze>aYP)ua~b>f9k;3hxU~=U1B4J{hT~K%{b7-a7)9xQU^N2d4-+sjLS|CK!`nlxq}E=8!J*Xhf)7d%oY4FC{1KTjH1ce$ zdAy>M$E1hdm8&4TGi(lzR`|<)T5$;R$`cF6D_m427U3q8p}&0?GDCBG7TOoXN_OD9 zzeua+^@kMDQ^h|RX`@QTyVmv1U#iDz;QwTCY4>PrTAo|OGy8DHbb3=RA{$Wfe8=E< zev6uQ#rg!p#y)Wjlk%gb-O)^xMUVZbAj867l8x#Dd1#z)wPZyIzI_9Qk`0euX@C3c zN!AB#ADEu2ac-FOSqWr*Pk>REG>po)*%4Af9Z15^njJTWI&$R`4*-3={L|-78h{@N z2pC{$tLZ+s%0-mUgUUs`aUllPXSs{Ab{(s1+NYlADHZb(XG z{4zl0rC>i`vFbqK^ z2-Ge%f|{`4qohVlcB1brOe_w_>Ob!|452@J=^Gh=!tvqm>P~|X5O%##FsZ>3VtpH2 z^2iDRZe+mWbV{4S`xa-6$7lE3?xK>CD2CFWQrB2aeC-M)RxVcOINAS6zuFsJCmm}w zpj^xw*Yi4(5vX!lq!o}M3Smisfpmw0C2mw)f5cRn9qzQz-T+A$$vyYCrCHZ;oRaEf za_V5v@7Ijm2|R{YncB$FZh<=C91!f4o)-=Xq#M*$y$CrA7=F2qh3`t@k%;Fy-I1u=Ecs9ZN@rU+4ea?7iDGcNL1J(oZx9MU^qWxAzTFzS|9Y z@YJXJr+7gPL1u5K@$A{Y^LUMf!1sqONKH&e_DMULZx9*5$5w&X`q|?|Bgv(7tqf^I zEUHgn;!vn$o2_LUm68H;`{(zR>}pTs;lRUkeUMxifb2fT3sE*9N4KYuF6bdb)Er)(_RBtk4J`KeiW%XV$ z(2qh_HKSl{5CX#ZctbYa{{DVJ`xVqhK&6Fua6-;6E(|t@Kg$WcB+OEm0wNH#Cbd4f zZ~g@ndW&BY-%iRA93Q`SJ*w}%LWRhWcrh(_m$>ok;=9oeI}oa2EUn9QB5*38S!?RM z8BS|^gdtm~Z@6288+Edtgsn;t)1qt@_~h|pG9V9;{_|_%RjsM1=|f%v>%*1qBmqk@ z1gBYLB99Po0GU(GFP=M~8ySue3%G7OUPj}h`Q(8E*x@l(Z=+`WKMFCS4^FR$(9b{rHWV@XW)Mpk-Pg<_<6Vk5G zOXseGdy2|GeE!Y2&LwjByN1kzO;VkzlXd@3Dl(fOeU+1KF4tyw7HjT3ifN;E+u;MW zrFNe-5lKbcIy^QzNBf$jf!L%J%+iARGCV@Ue(6f3X~6H3fY(e~VqyqTzH0rME>?!%5=12MH^KzfG+)Ae?^?7`S7 zLcm}Xii$3+EWb6rN_oYxY)g&~PNk@`;7KlfM|E=W;($%WLx^U~|iHh6|dnQ5WR8`j{26uDI-D7ge z(3ju1$n(~9JC}TdO7Hv$c^Q5y*W>YdF-WnrXS~!jlZ;yAXhU z{@0<66Xm=t1JKr&%lzMfas^sZz;n`hpD#ntJlx$4m%j$7i+BUT7$LH|N6eCBwf{XI z-}~mzoABuVkr7#M?^Y)f5iR<9Gkz-YUwAZ6LVQZlb=#llDOadwOGaA2bUza?R_v^!31nv~P%$x8J#PmN7s4 z!@l;1*4N2@}FjHd$u?20J%Q&q4xHR z)(XY1q|U+|5-zfBk|-#LN4d;N_Mz0;EMQ6Eexw!on5kNL=sQiurki!*K4p82ws-1N zqe=GiXqW8Ie=&1H)NgBOTy)eZNRRyc?<+p(zy;Ko!STj7K(7Lg+qer9u;#yz zY&(N-ks!jKt+}SP8dEjqU(A9C$K$-C0lF)k9|QUufs_IF8Sh^5X#WwyQ$aEYR1Ic& zrO%Wte@Xl~J}V$ozC0E}0RpME-M@03XSpiz^|oiaLF26ZYbZ8bqZz`j^agi-+#HXv zdETYBE&xnl<4#64R{j8pdd&IvmqR?B-~3#Ax;iyiDytM5^UO2{AhL0Bfr~93q{+!R$t9U>g(;~qA%V{bwegiSX{Q#+kssNfl$_=Iqgqi0Kq-WrvFG4?Df@olIWgH z8G8Q49XbTFa5)q=JIz0wTjOv_pF=zxDmKqY{>A$<8W@qfpgF-E7Y)q}ARl_O1O%^! z6TU8BKoAnYOqIe4gU(>0GcQ2PmW&6tw?C_IY$T)n1{{Y)jItiee<^(Aa3>)Q8L|){ zPGJI4Jk&9?U!q?&i6Cc}dzd?_`2NMKp1yB_GsYnPuk*x8Ha6bzHeD?RX#sK1f%=@S zcQFw8HoB%ed8j%~K`-ii+)~)%KmEYBI9wDOwU;T{`J=T9vj-?isHWxO7ETTA-EH`n zjmTk^Se~YD3lSX)<2((kEu_p7Vq!oT#Rz!xAYqPE`dzkMdq{iU*uv7Xal=ArVd#tV zr%09fy;(ZWA~dHW5$8IGb{u)`k}n62o+QKjihQIo@Mz!<-;`wlV2?u3P9LGYgp^JS z4-OPfSU7EsA_2Q}J?ejuy9@rH;MVtrMX}XmnH|{^d&Q$8<;7Foh8v^|*{YcMFC0~= zyFCEJT^42<`}d<)K)=xiT5fdlD3)Lq+KEwxI7!LuC36bf}V~KrzD09UOGVXV# zk2IS~WSsX}^rvFN9zmXD9G^#L>>jbddU$rOKp<|5{}-Xb04p5mW*L7QH1x1uU9n=4=zo0(Ry5Q9!hmvB5AHZu=oEw+Q!?0#%bL z^3ncQNiO7P@>C{kp|BsAiKA`r?b^(+P$n32g>h#eo=QA2NTB6iFkL65(Iimwa=%He z*z(blC@;hv<{Fk#Q>pOrbTP_19PFGb%d?9ntHbzopnmA(r|SQ@FS%G5~SN z*Rg2?6o_DzywqgY^!Qf*n`=~LY+=l2Nr!X48=@$tzx(#&XBP;5h*l~D1g%wPqb|+@ z_|pGYD%GV%thRuzh1mt7=8bTu8q3+F>UvlO!_9q}PI&DXHuGe2!gioT$e9B<)9NVf z-cSQXaKX9P}L?SzWhB2LBi~$={1DykqnjOHjb5kVn@=A);XP(>mO&VvVZ@EeZdlII=9 z1@lhLIZ9H-%wf6w{O04RAb()1x=GaYmH=X!I16+k2CkYLonKGr7vhT}lD^~#)MH&z zp7S~T7*{=IWG0laQ||Z0HjWw;NDqL>`W4rWdMyqm1mqo~q^jah6BSoumu>P_jm@H4 zPEqvNuiVW<=drMO9+(KHkwPJm@6IC~n@ztZI3{CH5p8@uteX@QwSlBqaJZFZrSDn% z8(j%+Z!v(+26su_u*$r6fr*fJB0=SAQ5TDs+#2K*ITUv6LNjbi_Z->i9G7841j{@R-u9{0j#+ll!1{z zUXB5T_vgRM+bDKak}1ecxds2?PjU3P#Y0(`{!JM3DRt~qhjf;7kyDasE?ZR9(3AC^ zH1i`WMK>~?h0)i^?Q;v?3Il-htcteVH0b(jzfI8X=sv2%voG(y5l~PRI1%?D&2;{< z+;D3GXu)&ey2hbia#SSm?_xcZxZ;_%KU?h{vg874#_9Zs08e)a8A9_1FL=MMP0&~u zcw*Ptts2NPM$YA;UO|%Q0sT7p47bbp6k9}uF)%i=gN?N{2GBhK%Ix^+Un#}K4|UzQ znICD_69CPtoInswT<*kF+#}U@y78H!9c2Lh8ZG=mAS$}7qR@^Fv0U6SB${t>b#3n+ zR!@Eyo_hT}R=@6<1wAw@7OezEc-g&TEvQ% zKLxyLjF41tQGWo?z;(paZ)||Bk}P6%ph*szpVtPfOtHOLA-SgbG3(PtOa0b?`M-WX zZc<;Ay~X1?!%$TzX&AI*P3CFHNdmrtfSNkIxmnbF4%`tT&^9Z67LGr7k(Jf_V?fRN zL-vM^baK9}po(U|MItH&RT2w;{&S}7#(L>}B4p{@P!Xl)T&*==9HVH2kn!>Htri+# zz)E9ZXITVj3%ES&Buu2LJz%clHS!2k2zk?Ca!|CI|cKYIKkDkKlHV6~~kmm6VhsXoOVyB|paUjcmqSl2lg- zxU3uAUhXXDMDDiT2MuM(Uo6_2K$gI}f7L~-q*2MdHdLZqBM}8o#v^8B<42apex>6* zB{(vzu15r*>Y=v+OSB-gCZB>9K~0W9BIRz4qFDl}3k#IL?JtL&cAQJQLB1x#eWSWu5Uo6d{KJ z6|!_s4GpBWu)%Uz6;=)H)``4nF^%}W=a5AO@Mq%gfsd4YoUdf=V}Pmj;G1vZK&0VgcO`0Ff+hvWkC!brzJA=>RAIq|BkA zp#^Ws={!9>4?qV75+qL{*&nEgpMzN;{&ydwInm#?Ljc{rrP(EwCIreJ&{PSdng9W- z=Wnm0&TSl~W#w6J@LLyv>F*>PBJCsOGltz*L#aZ<7B!l)i;eFsJ#Q~IhE5rGhCiVUIkGw^ik>mso=d z90+cc$KnU8xyPr+)&~eydjh8js;!NRpcz;)OhDVFj7PZYSg4I$M3oF59tfJ+!}_hKPp=Eg#qa>BhnolqFHMd3d}tl z*a^!V=v|5geOYYaV*WVUE#0uk6`PH<8xfz(xhQe}xk$c7J2hA)fDR-CO+fxYRREY@ z`=hm&o_{9ZOxN3IBQ7Z?zC9?$c{eCs23ue2azY30P!>cd1TKZB%{RV%0*l1Z(9mwB z>)!SC@yOOfQ=Q5;1_!fXQC)}%T{#CO*)MMW63{+>`l6Dt9B_rJLFBs60Lw4y3uo{1 zgb8U8NfQPn?saz@Ke1jQ0ez=LY>&Ts++4mF1x#asUYl4?G<`Hc2wC5oLGul-y?E-L zS$;V(b`j8w)qm2QDSqq{3h(0tPQzxp0_jbewuF_{BRm>GUp!i22GBf3?|sfV{8=Pl zjSrqG2iln)7x!Cw_Myaj@c1AMAR$X08;ligdCMu4ii(Qe?X?@CUmY;yHQf#-QpU;o z1EkBiH-OC;{;4LKIMqy&oH%RGu0sIUNVZwKWyYA9-BLT!HQ;6unogkFw+??tAaq}n zZXYlhR+H!nP@s^IMYK-=4LVroM2Jq2L9R9gvxgWkK-pz|1Ow)J$!mG$uaL_}RbK^G zj}TFJ5kJss&?DsPKU=*HxAW7pK;J!3SHAvrAf@=KKc!jwZ6si&A`Ovj6%=rQVhh>F z=Wg7EB};|U;waDboxFfsyoPqbHVz=%%t++TU3Y7yfQY!i@57*Fh7@pp&x!vUf~9wJ0AG|) zY5Z7w5DSof!~od`@N)yJCMLjA9E^b)geH~9ty$X)botUy0`3&UG_CiOR!02$xJ2w% z1fZJ_Y^|_{8WAuGT4haMS^u?WAz`q+f?_+!0S|)D$ee}jZu9z)2}!&EUV@SvTmm_` zIv7mh%Q<$Dfx*8YzeRjZ^jB)iF^5_^kNo!zhdZ`W7C(F-02JyGpTSqKwL*9w`_O|5 zn3M06XBHKC3RtYN8}ELjvx25JBw%E5Hdp88(-i_zlO{B=cyAGngiHgp1sh~ZifY&g z>U;=MPY8?|Sr^P6LK`sojX@M^|pGn)V2IN%W|&Sx5hq>S@vc$YXh83by5?z{s-Lda)sfm++} zUS%~mgzL9XNnhq7AR210l)A>jUCoNzmL!y$(;rmRM)OXs6n2+ z@FlO6aq$ZeUsqlPd5XCj#j|ADpuzA>|Gyg}{4Y6L9)I(H%h9@4PNQf=kRijVLaJ8n zzyEK_v{BU}dW&K;6|sG=fq<*=ayclYkbv6lB||X?Vh-DM+oxcemWU}wBCeO|;Praq zDhfW(!-%p73UO3)boH%DAjUw4prWDm&)kPL-T$lk%=l+l0tpcQ0C5dUm9{&U>}^#_ zK*~xYLSo4P2=2qZ;4D?MO)gxZ?;(Wlse1Au~#f8~)1*1$9P3YO8 z$H?R>CY8AGa9^NZ1AVfFK#i-iWclb(A3kUs2>S*oJ*b14BJDw~?N2(Ot@!eu555CR zJ|L0p%-g~Ym&fxc(iQ?WoRngJAwu^jM}X>Dcrp0^2uwS>K~p)Ps-aaZc*k*mnQD~= z-8DyGFAN}8lm^I6JV$1y*r<_s1wIbK3jLP%t56c+9zfA}mN)|D>TFRUi0w+|0ashK z21cPky)*@(RB#?(e50xQ-$J&RPlMxUIdz*gOx_?HA~urvM`dvNj6@pIVG3j@LE+e6 zPT+mKQ3naYTCjo4L2rBW5bRY_MQ??U*a!mUs@?bI^@Z3{A17gauwKajAw)yk+uMUj zMg}!CHI;rc-#Q<`cJf-v~3_4c`5N;H}ipjt(An@1+N(q;OrossQ*sv>*k9q00KMVktfM`%i z;x{KksMf$@TLFFY-9YU4XfHh`#;=#o0|#8G8${g04h(t*&^01{d!GH3BT>ois(op# zKelgZD9(GYX`Dh&-^V~P6(&dY$WLn78h{Y=0&Edt zV8D0@1Z1hHsfP5cR-2O22npCQkbn_GVY31FVysaB4>8sP@LxATgEi*wTFw@U0TN+T zwfkpEk6grg$E@bDrk2~4KA=gUkVB9^U{x!+K@d8l`1)ut3>yRE_`}`+jAI2|Ixtj* zJkZbAgpdoFYzGwCfv2~&3#|JH$SAvmC5e;m@4dzz>0(|7pwiRy90EXI_|DHdTRj>< zI|x-|&;RJDB!GH|fP^Fv^f()W>rPtbE!fu{XH=zLXoJo>WC)PqW&cYyMo9+zo17#{L~6r-}<2Mh%G`dv+}ro8ke11l>gNcB3S#c$yt zyk`NXYHQ}p1CW2^O2zK_1N9u~`p%|50DVN9AA!X*S_CULJagPQ&Mfg-+Sx=VoQXnc zejcGpa5r#-8OB)W?%J9+F|jGp^IJ~h0D)Yrjmxhe_khl`YiH)mSgF>N;@a}e4?QH3G4XyxZJ|j+mVo}Q@=0i*iyzKG|Qzq|H2ddn-O^87*zn!oN3Je{PqnO z2?^#rz@Eh)5SPkj&~Bk-8wiVI3jx2sUKUlgWL&hGIT?9_I~m>Q`Ad86sX;# zjJ0rFAHFPA&jx%bFbm$9G=QRlUut0JtMazAbW$ZotYQr)#W1n3 zz(z#QgEPY@LPD1f3}Mhk?<;GB1`yZB8jweDz}0EIcVLHySf7wmIe06IUg3;|EqlinJr_H z8oNChK!jjTeNhawdZ*RfEC?Th*u4TeFuH(|!XaIt1#V28D{~xdxWqvx=BXk2|J3&0 z;avZJ+xWX76(yC7gsjY@WJQuyq@s|$l8gotGD}oul2M5Y6*9ABmJv#n5tR`#v&p{C zXP?h?UH5ey*Kz#r`?!za_m6L%O1$2$*Youl=i@xjhufdDwTGe(IHfp)EjqR^I{_A1 z;KZS=HSKhG1K*5QY&Qd>{|?3gqWkupntT7@L)f(4G`V!p%)bYQ!m!yYwekTyBD;&x4hNW_2t@$y@j zA2Fa}L*;if^6md`-c|p{?pF@49aphl|4-gBjr=Ct@2s4hbHONQ+~>synv!W48G|}E zg;;=j)-m2wvv%XgY^15%FPU)Y6`?O9mRA|42rsR%;3Q+EG!7Hj$U(dDXptZRrO5M znmR~np`(z@!EuRw&-Z}as%1oRp@_88cwRjl`b1`Dl&RlMiQogKPw1F8z$AjV?d|qC zb2vWGUPGama#>Z7KFps_Z{yV!9sinDIB!J~8NPfo;&%11tx5)U{C97B|8(dQ6l^7c zE{M#NLfCOg*`O=Eh;6ifT7MoA3CgD>E3Vlc}^6~K@X@&&; z$;m6uuC8kHvg1`;XTYSqqMxkY_6$E)7+u-sAzz8fTM>H8$p-(h*igr;?BElTVpK@e z2blrI=GYtFdw2N!h}Z!P+Z7aWfwd2WD^R}y)G3-*2vWkAL@gR>IyObXQE|ga*e_4L z2x|DiWzX`Aj}$fy!>IdrLQghcdI`1`IX-oH-ZHx7uTv&&yUF?yoNk8Ww3 zewQ!ERyVI4S@aljy@KIiy>M0sgGoj>a5LH+(p&Mt8T6Q5in+MBR8>_GohVEz&U$)q zYWBMKimezSPg(tu_)cNrW~xAfE|b{#yXEdefun1vsed8~G8kD2Lw8>2 zI+M~MC_vsE*4ON?eXAGOkb|3Akch{%*XHR`0T{67)y@3G;He>dS>2_DJFAp_zJ%p( zmx5W}%2lhFN2?WaLiBSs!a_^vY77vIP8`ECknuiNF=AQ%;1dA)I%s#XHs^Xh{? zn9)(O#}R(B+XAF;_iQ+}V)4n{ z;$k_ku2qdl>`c>=+TtDy&QMK;4T_>&Bz7HW?#v%6hkU=>=cTh%34dh@odW7T`M=35 z&tp*^hqvwzn)6BvtsgX{enwRIbKr+IQAzxC2uTk zEE;#O-dkFLPGNm!+yc-pB3BtaclYr<$M`H@^lqc2)DWevHgf4<2xYf%9!$;|#f+tc zh>=oLMNp#LATRar?*$Yu;#9HlXRHMs2y*i+WGe#OKwLwnYsn3~oR)Svr!r|DyhvOK zU58+#-(OGkd-^BOkDCW`9aAiyg~3P*V+P&w;Fw$#)3uf4*CCp(w>+DdlFn?;cNz)m z>pTAj9SG{DHHaPFxw(>|?yKeRD&O4TeFRi>Z;rKnKMD^}7OT?js^5kS>jTTY4h9kz z2M2|ShQZUSY0O3j-+R{bZkr>cQE~O=iyLys58L4T2oS4Ne%w7^IZRnu{ycs zBQurdj2^y-{NCaQt|-mjv1+m>|mA z+wVoe=8IbN;li1>_juIqhK2dyHgWOs9huY3vSy$_w#|U~5%n5cCNE4Wx_Tmvl|MgM zPlsd|#*BLL>WUxx;0^{Ogi{aXHpytsZMLAqZMI;D{|62&Gs}LsK$yu=v$SsSmlg}A zjak{Ssiv&keSyfNnj~5vB{`mRx7L4z-UhY@?SpH|YC;qB%bKkAm z!&}J>Al7xv&GvrZBBCDzLWD^~{ePk6&OX|&>>7f^RlzGhSdiX}628%BV$Lc42Nl zc-p6Cu$M;tw4z1oM!#pb;qn(}LOOL2w!56&jIAb_K2aBeCU*FRLrVJ!satrz8JNHB z-??&@al+1KMp&s8*SkHm4T-NoK!?F`dPtW9X*l8Bh>3~0zaEZ1KK8OFNEQ9oU%FZeQOxH(><~Jg9gbea02|Yf( zEPsh_@)Cgnl2jZ8W^QkPADi1A_6;2*F zT31^!6?KDCjcv=jQ2{= zz5pf;l*!mUxFwXpJRx4=2dbH~78Xx&8W^P6o_GfG6dtr^!}Y?{G3=ZQ+kvaK-3LAX z-2d26WeQC$X#R&bO>&(D)F1X+?my`4PJPw(8{f*b>|eM@`L$5U6kMe}IcyV|8y@{Y zSlwVkAl_Gm(!FhK@`6dg9h{ zdPL8#|9My<=n-U$hNyWGi4jVTeuDzVKiCnr?o};Fg`LuYuA}6A7FpqLUc@y+$N!`#eaR; zeKqSp-z9&JC>W-6f#;FWj&_1I|-#7XOST>6%4C+R%O zfhc+F-8)5maBBDYX{yVq1`m<0@$Y?s96W`pP*dx>Np9WSWgv_)yR5!i*R%1y9f1_$9U87nw?ZaXVp(S+;v-qYM+6a!ab5=XgT=#^K?rQjD8yxCVe<2d@*nJsjFy@@EpHxE z_INJ2g|_WDaP{5t(oC^tn{v>JY~xa$23Xc8dav0)YFMPsz4C8X_j2h0 z?Sq(Hdx$!VqfeyMcg?j`UiWkQY_(PnUEdk;Ws~9xC5NLMgcgkiPPsN2@jYfSabt=2 z)ly+BO>tj7@RRS7*2_&@udHE%6ZA{^+3?5g^J9iIncu%|+p$ktPZ-Gesf@tU;}6Bw z8M}>99h~dqB8*H_9q=U(k8x@ojVqjCZAK&hopO@<(LaMl_Esw$~TkK9d;4 zu}i_zZ6>j29j|?yrgMUzxW>omFzv!}=M@{*6m`GE@wOb~y&D=T9N$?HxLtF0lR3!+ zXuP|n15PE>-VQevt8vcw0v(Hs%PHOpmqv>ZJXDim>uPtMlv1^RYlyaOxVou5`Ml@p zNwpiZic%_`2bjXOE;1Kn9izDnOt`vu6kt>3myuGTtbGupRY@ z0xtfl`<6L1e;8;^h;Rlmx!#YLKCF9YAh@eaWBrGP^dQy+a>XZizLOcX2wTtj?Aciw zXUmUKcSMR8HlzP=#*|Lf?A=0zn2@1}Eh)d+cGZGM9zQl^65 zbCU)F!x4G6BW(U0Q5%xJZ$FnEr(BNfb zNde8m@;}zWPsO5R3opWKGeo$9%8R_!Xtao^HXYjaV~?d72;w)f>A3uFgQG#?)e0X) zI2m-Wl!kY+nkEamX!8GFl!N-_*0mFn%^tf4wbq0SadQe7#XlEem%X+X?b{6$L_ut_ zdR;{qS~C-BpGIlxS}sRaFk8M5{GiFaH*cF^+DxRvNBX9Q$Qhsgs!y4DYnXLJ?QYK8 zj8|punO$j;aMtw9UE}nr*gLlq{Zs=-f51%%BaDZW%fRhAm*z)A#y_VR9?)eO6bg_V z#aX=KTa%l8I)vv!&D#)Vres6qVYE4FBP-Nm2kb9@dGFnmyl8u8R&L zWj%miboTUEd=@|X{_La7rdSm^f0|=>; z_m4F-q+%NLT%PeHjR-p5zugivy3W7ze*Mj>E-p`T6#w9Xt4BA>5W)^&0+532#fx1e z>Yx~nlRyx!6>t^5~YuBwSjgoRW_FLkF6|xwZJ5r=C0JhRuH2)i<((RN! zov?lHl$Ms3@b~zGRTM(a0ktRG$SKvR@NVd-D?UmBa$i>(!HIF{dJ#J7Jf!2t2IW)! zKWX=yggyUqGa2Jp^>LkHpL<)HYWeFKgVHwnWEEWz&1pZFyFTUi(L4N`Ct)MP$;-1Q z<%a!;lsV)p2jM?u2O7ZSa2q2kKD2YI#4mokZC)AFnbmCc)?cLPHVmSYTYg2#xa)&@a=E{O=gMHy2KaIkoim}8>lnFEp>$!A^N@-5n6r`r^wdyA zI+_reDG;^}J}D{hwLy7+KK3VCD{-l-==Vq=2&pv`!t|+c#@9D|{+y1PrBw{PPrCp| zYaY^#BJ)^S*j6kp&J(!}naVZ1oGJCD9z#O@`PDL}oYIox=`5_lM@@ z-^%tmo^`$UmH)xSg`^p8S#6uN-iVF8(``RsRY~!J_f&uO&KC8)XpZlxnWE+uM`AkH zKs6Mx#LmFrL)>KX4mkHcz2zr!aMvzs+`4kiO+O`sF8OPLSa>%)+?UkLAt51ufLWi! zyF=g~;>Sh$VAZ1`4tRf9*hQ7XY%74BKEhzPr=gWoz1d?_VfWtWO(m4mXSW#W|^ z&P3$cb{kRN-5VE5wh?S98aTe*d$caXxMCWZDJTG2giPdmYaG_4(6IkEjbM zo7mVc+UrBCj;uy%Yz)2$ga-;gE|F#%drWk65C~}al-my;JlIgDW9;sOgSL_aLfH)f z5dbT7qa^KC;vekKll~U3#)!@y<`tT)P2eAenlMtxa2IbC8(t-tGyDO;gealzUpuBy zvXO#TmGBS=>n1@KM$@8Si_x(Y{#j;gSG!t8&zZu)LO6u@C+VbaEARxDt^T#;iOh@N zSrmL}OD$vE2@tZaKrZY={4FWGiyIHc6z>JP^<3nBNS4YO+>t9yuQr*y5>gv%^(z#T zG#$pV3$`Cf-P;$})->1PDEs2joW11;gJ4u6&R}G|tHAn9s*?qY7g&Z?@{PO5T7G0D z9XXPL3c_6H@Ngi`<<W z{YqZPj6@V}OqZI7zm>N-?*c@bcs|#&pS0Yu&CUQsOUtx}Lj1@`I>52(#K58aH#?jV zaizp&M*X3f=Xfd<@x|IPM57@sZ4W^$&t^1X_Sn(YWp;0)=^Z!` zl9Wx}4UBFoVa%rgd|=)D^mnz?e57Y^6@4%lRU1C4rnc|vGT3fPzzjME^wNtT_=Z?b zH1A%!GXF~d4a6c=nEyduf;*h_Nwn(zD>a%_gnGQpV$xTKZUYK=v~b$-+1hVfe^yn5 zMCfFRPlTx3oLeIh6<4$aQ&9e^p2&m&>~d@)ZzeX9G>8LE0xO9k#4(3b z(srph5UWOH=7(NOUhbe^#25(Q2|ZPHlEomHB5X5a(h80Upgm+~8R7=`6Oq3q&tccI zXU_=!q&i!`!wkst>QKvdm^Q{A>!+m<2W6l(tOu^rBEu<|m^?WhXokKJNSU&k87H}g z1m{A@%{%+C$AWJ#-|nX};qc}<{MzU-KP*9qdlhS0)PrP&SwXY?=e!NH6@yLV9s6GhthdS^`Y#Op>?t zG9s{>-E>#9b7Uz|c%hy@^w&~U98_OWv zpW7VebMxkzg$x}e7fl_Rn!37M!NI{-^f#?mR8irQmde!2u zTs4h>14@$}8j|z{jvi8aEhbb6(ivG}t5@ z+D1S&0XZgHKyZ^4nm$z^D?$g7un>uI5d-866z8#WM*poTDXM_?hca#ZrOlbVOO6P;Hyi8gk#>buf7;vSsCfM2f@qpUU%#2U>fMHOrkmTb+4lhk0f$|P@$2I! z;&AN4!VmtoC+qoygcPq`lZI`NKQJMC`e(E=`{Ks0^j}y0z=Xd8 z6)t5mkKcFoDC~_2a&18~G`s2ROUg>v(3AjWP4=h-j_>cjbN=oRIm6bguZDLT=sSHF zs1!e!)T1?=)so*3wvKjj6dMY7+b(>i3s=qf>emoTHEglduYg5(chA6f^O`5d{p*sa z7nCCXoM2sukw3v&nf6_5fDq2fX-i5i z@`D2nHGrK`2#@`?W^1=%8b-t;LPl?xkl9KJD5S#fz7=jZgfOM`pO)nA&z@;m+`Cch zMqqY8F^V7L_b1!i!Z$!PY|eJy?rftoXKpvSB=2#YKV3WLC~$CSoA(Sm3!UK)Lkkvj z?wk@n&zvB-Z&_5IdcrR3nDvXC{N$X=u$5EPg2mc&KgFas_4R0HYPxvR+l13cvolW4 zS?3FXX6e%G>JZ4?Hv2dgHXWy&)P1zS2<&aOtl4|PSHz1#TpFzk?4kv~#vs_R=jG*P zUsO)0p6^MZkN)Fg|ue8 zaPqZ-Oe+iR9`kO?8;SkCucIqbV<;m0orF)9@xX?eGyi=^rRMcl}n)m4mw8 z$_^t@r`Jhu-L@?}Er(W-gLnTL7b;G#jl51Djz93F%MVg09k#6r&RQ8*&sg$mdrrqd zyk<8|$NaF0LX}9#WVhtxqP5(~Q*sHt<>d*^>O-`wKc2QQmMVSnoOK;47V$h${;C3t zOcd^?NBUdThs~O-aO2IO4iiIjazaPL` z_sKd-$$F*{!MSDb1}35^m?MS^qe>DywZi7Fcl(uMaVf^mr`B+aL>E0c!iTs>XTnK*X1_0&lie~*LMXowx1ljH&++YY+^ND*k0OZ9Q5w9 z{Hps>xewfgy_P@L#|3UrUMlGG3U`(oWv*?XqPJd(XfnOL;JKsIg_}xZ+w(Iy^vb{2 z3v?8Vm_F`Z_QfTP_2*q;aFGk3j0#Bq>``9DAgMRDnzGbt(682lTMh%-8xhB6i#0CZ z*xqW9^A=@M&EgrA+}h)c*}K}Nl8}^k^6E4kAEyn&zl?PO_X5>cYoEM(sZ+H>u9XNZY>lW~aVT%tnslQRAjixHQT?_SSufAw0|Dou+@FLQomq)h+2dD>FrRliIN zy;jza;)ZUgn=6T|@+f_1^`eZg;AKTc9zbklLz<{vmm;Z%5a9VTYDZzdytDafH4h9G zQ&LitlpWCg>kat{qpWU_?x+A8HzX@vcwHYo(T`RY-=~wma$S1lhr=BT2QiVSZ+XGC z+Q;5`#UUO)Ao#(nmesYTC!NICe=kf9&mmhSA`4H6#?PO23p;zQeNvXaOhK40Jav~J zr|<5foGWoLIn-a)tDjZ{FavjZZW&g(oB|yNsdGmIyp!=pX-p=1yFB@d7{T)HV&>7b zoa0;VWL3PWEe9iLH!RHUQnEj@!o<4z7>CmGvfA$vv);}};x&d^cbGxpih(1;v+P zl&Se&Hu?nQY#?52v`i+CYu^7enW)#X+GLU+_r)`2P`y_|L#%~LNJP=}L)iq}BL7sa z9-23npgfP=6IyWmx_0Rywf?Zi0A|L=#S9*dk%{aUDYGBe2rXRLmiejEJB6LEDH=vf zqfb4~4_DsYIeOLec)EFA%OZXD$Tb?DlxQdnjjW{Br9{*F?P!k*9xVLUW2w!mahBef ztaciH6ojovls)M+idLMDgl`y49CiDPqQA;ntBj9rcD3Tk%AgzipI7K&&MeqvI}^z$rRAOQIgQiIhVcQ|)9n4TqEJnFgOX!*L*lH*LjS$alz9 z`LyrVM*X!=64VUmPjO`huY*2YX<1(Hv%}Wwq-q4;zWYBO zHcb7{qH|q44fWb{hOFg64|i2{SrPhZ0iAN~9bEQX0qne$Xm|6r`&m)?`Sa(d5ByCS zM4yD~8DV=isq=@}(wtzbsj0#AM!RKSos9SF#4j$;_h;HYldh$g8@cA5j`6m=P&_UVKhj53uDIHp5^r?kmcjPY*7p&?9m7s2 z<9yltFzxlPc33ou-n+%IHh#B-!9!e%X-CQRjLa9@_L*644j(>DRz3Ew&o3MR>rxq` zTR=5!`TRWhggY9ovxry#y)gk3km+c=Jd*o}%qivV>6S;AitYfs*@+D!|1zYuN~K_D zg5KHv=ku*j3THg*vZ5{5u+HKS<#Ai#CfBthiS-3{_A$#e3DwlSmL^e6rnODi*`5ql z+oXr2|7pJyy-g?gwMClc+J(O!%BKcKR_fM)mo)~17~-#S+39b8@(DoB(D$T;c8W>_ zdIc0?nYHt&@WaKIrcT;no_ZT-LKphjq)7)|HIRSDzB{#dLIhWd8t=mQD6{uhhc5ys zIg@A4a}E;>^e(Dx;1UF4BHIb;p0L4_+^cwY_vo!pv6anFydv#gn65QytjS?H=%&u| znWa`Imu8A#xR)j}{>lr?>xQ<>+D^qM30xaUcmV+p=`{)E8ekmVz0gn`oDKRCA3BLX zgSV0x_@E&^8)_b*qNunU@Qc~tx%)@5>3{qy(0U8JicY85)w856)5;rNz^RnCcaPPI z^z?2!Z^8aSbYqGhIA5&->%`*sFuf>C5KJe1GBd>pEQWnihtQ&qb$HX6L5D#wsHdN_QV==AfyldjGx@h#URp%@Fr^f?)LE*m1gb>*UenVxQxBc6RnJ$@+}g zza*6Y0gQ+;ybi1Ig`iHDV2BDuQq+}Va7@w6Xkb)`Kkfh_TX1SYEYlOv+xgZ^e)QaA zjR_pj>(3Sgk$r;OcQUCFKAOx$OLI1%^MR*W08diOZ%V`9X0oq-6%?psjwNOveed24 zu(Y_a73(kf{jR)x1!bFvQNaA~0y+S=FTt5!L&s)dHm~Mth{sJDY;vv%W=otP4@y7Y zxNhA#LO=l+O2}U3U{NdhluzXYLhB}X#tSg&(#?_dJil8o`N2kUMO*#^;X4SM`kI<9r z+R*1Poce2PRTJYgzK7jw^ydmX)`Azddn85Oqk66Y1f=W1!F2GMy_8}9QVM>#LTAz_ zXcOP!DIlb7By1<%`R?9cGe88u5ZKemMvk~RlTT0NVcoh<{L+sAKpZTD4vrOJ$Jiun znuJLAiAI~4I*oyx%)$Tum2!N&=(Wkd6?lSV1aDFTPbLY%J5hF+l(0k$@0JVV`Elq}?q2?B^R$obvUwC}6-Xq2CfXx^itl%f_H zANWe|HTT2og*_wJup(qYkn`uXXQRN!_RMP*lvNWZ-oMi)jna-k41RUOk^xJRx)WPL zCtoA--R53H=WRGK!Q3WS`kgpz-IIT;DUF?Eo`;Dm;XDyo71GLQxN6|h@5p{Pj9iY) z2Qu$dp7R7da`Tog;CEj#fyATXYM?S#px4Thj#6)O8;GYEIUZ$YWuGv%8v2&ErA}rd zU@~{NVZKS(&2@kAaxxDG&z$WVQ1ZA!xrNbH1_lPB2!?9EzJl3I-b0+1Cvf46l=D2u zdpzp)i|A|XDQQ>wHK8)j`cee67tX@;Y}fjBQc7^_0AkTBU_9kKT1qX7drk(6K(Bcz z?iMfbDNJG3h22Lg>2(JH;96zMfba&)|J{RqmY?7yOxOY#1IHaEI3<{8ea)%tU^s!~ z`Bt@D1;87?gD?*f;sgK|Slm3I^gv`%zY+a|b zz1Ah6C$-fix|sFHr}kdsLyx}$5%vLcYL2+t*OnuK!5Kl+;Lv`!)e_|c(VBr|Nal2J zCL7dpJr?ud^Vo=pi15E$>4o-)i<^5jKzZJ_6r=ryXI{W8Sm|Pnq+OYrJ{`tQSS13E zxem+Y6p$Ln$Hy;&qJ`1{@+y&A37QHo?;3)i05!E8Yh(RNZKHmg z|Kiz3x5+BKAs4Zunt2tS1s|+!N?bMhB8oAI5P$ zOg*=2?Tzva773Nc-)3V!_rt)R+HHO~N9lKx?(6Y*8Q~qx&W~mnpYerXOW8hi-L(DQ z9O3UH79NHI7-LRfSl%IRn1WBnkl1I^?__e&L@T^mt6>nQ$9Zw88Ij+x`_BdzF+Qh9 z7mhaduf&O0g3=0zoHq#0pm%+B-)VNw^O>p@SCZ=LXxdRfCjb@&OvBkKs)pAu-&~Sd z+Dm(AZm$7-Z)}^9^io~|2&b;y*Lr04Dtbl*;_(mfDS*!{0Yx`ugk}>vcuS7m-gieGfkqmfxpxE(|NTvBs3kqN ztdc({CWald2>t%%MjUNcIgdA0BT$3$A~oAfv?WT)g;TaZeyLeccTFwgquSiOZ8Gy% zZ|u>aO(uNh_1+t7GI|ehoz@eM{DdoTm?kj~twmZVRoUm{g5cU4r1tL0qZA~LQOAN=sPAr3A^9|I0=BNm^&A3A^ z5Bk_`&~;5Y=PjlZl~x--qrJ1#c8nnYfnr>pWy`O%PRL+fM#5 z%(&7`->yNWr5pdD>2$(i6Q;`+OC4vw%gvvQw!RDb1nAP&HSD;O$_?`~EA~GBDE^R} zS|w7*LZp`Q*Sh#f7yt!qm)J<;7})m-lW#B7hpDx7^&rB=J%4W2 z5Ol=07jq-M{XJNdq-zJIzMdXaH6IzFwwa8GcgRRf`;;|oLtV>^|NW(hB=!}$dVs}keaiQ4a7TK{Wp?9nDRcgMK*n$w3; zl6I8a-E?Sxbt{qd@{W8Z%fw~GH}BqEu6|g6h5JC4%`VPg95x$AK%f_O89ysC^UXo? zs6TExAynA2`M!FmY4sfc>dEIYVNSsl(b2O3&UrTuPCRL&Kd2vO;C5q$%GavGO+H+` zBMy0GGRwyJf?rzDwiLyErxP*{-;Rmst<(8rkm!>&dWiUKtd2 zc=$ciw7L)C>9X*XR2S1yZKI=E6SxA-3Frp5M8>YsSH+BX&&y*w^i)2!@89)T zJRC6(SVv|vi`~qy|REg0QS#N8cofwH1;j7H!6%vqXxr|G_?AGyNTp3%>hg)!~e0)-;>A2Q)$!mYi;zjkSG;I(2 z-PK*T{5=0Iu~}V+=@O@q^77}sSCfvWJ;GguqubSaBkpI3^4VT9AT5=Io;av`))kX7 z5-9UFDy+A&tEVnpQy#|E&_B7wO3htcrEG*U;ix`o@$|Tz0)=ny2hy%>~WpPErs=`n^`e>AvX_nk#_bCZDfHy;G4$|WBN8%fha}NQCxQxb>>`g}h*7q%M zAMc51p=Q$~roI|c*cb0#=w_9BMK?r7R%+Zi&+pXNJxec{emZ2n3YA=uGOeO`_QY8= zE5vxhUI>Ydyp0%7!ZMiIJ$m)tL!o~~=z`PZ78bh=wX=H@Ucc%(8+gZ}cQ*F4sITA- z-|AH$)8$5=I%(EQn<~9Y!vgi_#&)-f4G4zR;E3o;y!bYhyqM9{N#%uc3h9`MPK0?# z`a#!jW9@10H_!H;5Y*dI#`fOOzge?!eWK*V(Wv;xFWw9sIxUTs76cSk*-FGmvO7e# z_0(pmRy`w-b93Eh_Y&mrYm6(nl`oA7^RFD(y?Y}HS7^uXpjos%k&I+{-($1p=#@hjUBawAb;%4A$0U?$7cG`gLv|@FbEA(5k91aI zwk?~mhlPW5NAP4aXuX7b@BH`t{cj2jiS7XS;3++RhFl5FJS2W1k_ITjB;<-rk;| zK^R(>w70WEA@<37Wbp$~hW|&zWIsdgrzN;T#bVG6?))JvZnhiiQVLxp7G^k%3eZBM z*dV2n`frKjR>TWtwR{Ph4U~0s0CS##=+jW;f~wb-jy(uzY%SR;jMZS&6tX~*jOWQN z9N?Gu7?=rl5789xpj*ZxM*>V4%Xw-{xt=))gZ~s~;?I=}ZjJoe}`215q zCl3cNF{SGld&rJYO!$AVD=$ZPC{zMaf;^md?Vc!zF_6x}$#t3Zkn%o0JBD0-n)jGS*oNO=?#(eBf_{YxN!n&SI0 zNnB(FE6$gYTR{1!1m>mY_5&5SDZxkVyVsJgfDq9@MI?c+w=RZGPEHO03g^CkTL9r! zVt%~|s@(O3yf$F6n^xcFcLpL#8dg%? zQwT%G4X7G+BxfPn?9%yW#<(nzxFuS=z<`yd?*qLUO>X|fsj$;|H)(cD7GtPiX7=il z-|48yn?<9V5li*My}iDq0`IO0!GcP$#!q$ z*ix`uNM*f(LU>5fl64?dSHh6uHYy}@^sxkET|vQQupH17L9Xz~@z(4>VSpJQ0G--;*|$N zV?ru8QDpm%IdDsog=((Q0*V#V%Up}`8~MI$o(p!|yLOr5c9QD9XnDzX0c6IP*yi+w z9p|MK@X#0_MeROE-B4SuHz#DqRs)F)Mt9gjd^ExA8O6skhnmT+_6hf|lX#xVLI%69 zW?QKBu!QvtSXZaxS(fhp>Q5jgY=)^o@kG#j0yrVF@Wx&t>&ppwD+tBUTPJ@o%n1*btE)Jmt#Pw{VpV3A7dmtrYL+3 z)D-~A0&$(spx7bQnKAHpfpzA5nbs%W1J?VJeZTN+DMu*vA2R=@^Dp^P)m^;;o@eWT z-N&#KJ>4L-j&4F50kku|Jpv#K%uDQmj{SWdYI}Zu} z@dpult%`+ym%n^F$LZReV6SxTu<@fE1v0c!Nl+9j!(59@Y4C^%@&>MaCHf9uTF?O5T|Ekz}tmaG2ab69J1*AiR*6?npYT0wB1ka=s!Hp3(Ie*a{- zw6EBBYq{p~RhIt`RHP@RkDxJxEe6(P86Xp~^o(epklc6S1t>}b&njxQ>gMO^6pLwSwiANW`Kk4DqLV3N^od1QI%M7B9lr_`!pq$2%qIXF^C9t^-i zu(Plx>Dn49Dq<9)`NQTP{W@VFMzjPGXawJkByt*t_(PSM;)_!!XYu{xD2L%kQKcT$@6!i=-fbhZ5;hz3&zM7ia9G)%U8EqvM0?MLV z?vC#{m)J=mMIjOg5CFAomm_q5EtB1UQv+IH>}!Sp$`t>q$@1gcU^3@xwiS!7YS8wp zrMx|YV1Ek?U52MmpAynO`cUk<6XeO+&UEew%d(JR{^{xIyN;2@d#^q>VdRt6NX>{W zR#T*0{=WxOEEe6qy|$zyQ!wC;ooF>m0aaoYaNd8O_;anLrF}>CO@MStB&?vsBiL7f zow$L*BzsMa$gT)AmKZ|(aGOy7mhYJKq042JzOojEEqt4wU?n7nloT@=kD!ExDJq}79||P(F{rkFlF)*1 z30C=a(3`M*k6ANott(fX&KJlo{o6C}cen6gc^kQAF@byfxqp;c+#%sgUL9g0pfLtM zoQ|G|VnG-fDdro3m;SX);k1T*#b|9i59c9I-PxzN*vhb_jAT5NuH;xCw*JAWjY2`7 zlp~N43{^}pck63P-VXW2#k~!>%w${wia&fo=>QS`qaV~ePBk_@UPkzg=%masyZCEN z(wKA$#9dXcBK#YLbLV&wr`BL7^k%$TkcjB9ty9bFz?(O3{{AB<9{vErfmzgbXtXr* zJu&wEgKcS|7y%I`qL|oLz=|pW$OIQ!5r_j2LpuL8ZpL}@2W(>sLH565I#Lpn?0x=g zhDBbCaZ+Pfa?X!3auQH>5D^41vw44z-F=dt0Brz@Vo?+#)p?hXr!!>1N`v&umG%L z@vPkRIwM11^3RPME71^7nIY81w{}*+#w~nq=PqL|aW~J{meIvPv*Sh2HR~=d7&wg! z%osG4Imibua-3j4Jw4)Jg;g{%=GV?*z3{p9-q#yk6*U@)m^%QAxTik zk2AyJcl_kmZPrih(u;4r`R}HBpj4u&e1Z;`*RhB2_i=O?b<^Lme2kX$hv62%jd|O) zY}rCM_Aq44!`l_ z_xIrt0OWMr@_hSp2Q(U(V|74wBB(tEG*pNH3;bM%Xo{>J|2?rN>D(Ef*}nW-#vnzv zBah1G{G(ij{O?cY9SP5%Aga6a-^Y#NBo`P*#C2qD-rylL5G0P_$zULyAty)AKXEWL zE5#3;2rQm9V)5=wTI6w*zcu zCwnHl4uZ-Aj)F*rVZ5f+fOkShuoH2DoA5aEkG01ksH~M|A{K`eHjhwcL#%9nyC>Y` z-#e*QICA@?Ei{kIgw+i$5Z+Wdjrd zeo41KdoVYzHtc>-^>DJM`F9Wd{h56Kv89i>eAM*x8WvUBx18{Ah>Z#Q=v64J2rnxk zA%W1fRaN~AZiN^WxiJ&|H;fX=no2;hjHn+=!CH@V_gI>>Bs6al3CS75l!q0YYs;Wp zIcK*zqRNOC&Xg7(#5e1@MpO-)tzF{c+^71c`0Vyhd`A(&lSj}lLl1_W4>#Zv5NF5) z9c@jGBOl06XCWgk#e9(H-$LhUcf7z30`P;0BREF#gr$A>@C~hPWgLmG{8I8OS_>4L z%#S>PX^#)Kn*{1BbVt#Ko=AT1j9b4XsnI%qDs-X_`re+wV9t1u4G2_7JZvdnq;`6Rug(iEU9a;|X;rhl)1b$-3Q zUC4}pUZf2SlO;^J^h??jwFJq)nZOgI@x>=NoUCn*_=s&AA?R@Itt(<;E%M6Dqd%6^ znR3oH0#KpEVR>J-7N)NK?&pLd?#Eb0`S|gT6cZShkYQ;4)nPwK>aikLHRbIlEoR93 z%b)k%-mDS|N9nWfn6<&(4Q^juhZz<3o=P~az2octyTTp&jo1qizmGtugb_XX&2=?$ ziWaAT$QV7}w~4>9u~938?i?1Z+iI~0C17eM=XIyL^lqgMCJ4e;rv*MU3w|H;TK!5YcTO>+I9?1&xVQPh1*+>MZE{O7d%~^ zwx6O@0nvJU$7hxEu~~rODEOg z`#&Y6_3ppOWUjw=kv4x>ZiubDNVA?klfv8qb)R^Yj16pey7?lec(W2ef`s>Cha_fltba-rguv>bRSw%ShaHx^W;*2Op7u1 zYRaAKPG?$MTe*({x{yyzZr0&X#*pJGsx_Wg5t&j1o75EJM*BId=oC&<{#0s0YK2ZHmxud`M?VU)VN!fWiUx(&W6jpe|bzrU;!!7r~WU=fc`FFLxW>cGgGSfC_I%E|(u zmq1<9qu%T)!`9Q=E3kL3CMfsU;LTE*(2&0G9<8;FO^@V9uS!7Vl0gjAO?b;4QJ)9` zi46MLhv3OJ&bocxBu=WOg$8Lj5e0r6#C%{9>0U$1*3!|D=ynmGdV79vS~?@;=9)D> zCSAA@4lBcWko zjUz3n)2+gH8%m$sACwhx?&PUc=G+f}yP2~lZuh)*Oi8I+y}6}@dRZ)r)pmr+JizMW zMe%z9l|PxU1P(9miK1QFfS)1R(mC}R&9)%q{!0T-!B_*Y*s@i}+sCDYpzHkd)*0#A4WAT|XG)z240ZFw!v9TSw z)p_<)O|KDU6D#c-4{Iwc4JW4@q|mt?CypG!wT9L?Jbd`jEoEe2KvPYvCbCglTRBu7 zn~2H&y|9RgK@23SYiqyYr#OyxzaQh1;%;f8j>#D5m>9?2nr4=$#R?{7L*zlbA^=bzs`25&uKOrnQ$;rtdoC}S9PF)M+ zp1|;)CAHax)C%Uc#j$h&%q|q%+CYchQ&c09GYo70Bm6|e8Yxvin6ntu&|sdr|kmJlMevPD)PEYwjW&sgY<_F z&~e;>ehq9d1oDSi1`v1WfvFi>e`^CemLJq@h3LsZ5w#`pMzg@^UcUF8CJ zS8aD)ap?TzfZ29KdW0S?pIl!?{HkrHv8b;0wZvoJ zU+13Dl{!6F2&)4D z0Rh#)=2RV5*Mft8I#$I$e{PME0)MKBSx)<)aj}OXn!GQe{p&QiH)qQ|u1Qb#leZujvv9UQQXB<(m%1(Y#sIIB`j6RF|C<1owz~CTW zkmo>zwN9Vz!;9YtvC;jI5Yh97VPRp{P;9y%bn5%GbNEN0zJ-NF{#28GLIq%^wEJ8p;?jKb{PND(7y&`SsghN6v~zAK+fAw6Sq&WMrhg2oF;N z!pvZZ3R^wqPiQrF_jGr^jE`5^bv)|Rg`Drb6D;2o69p{Jp5>F3b^p{Xi?f?wP>_YC zgLr`bKye?+xMTn!*?u>F1!a`68Sl?IAUW&l=ya}L%?1*X03^Vx@T!$n0&E;!V)Stw z(Op1D2xH0@_wU{7a##e9`{FezZ|?HSO0pcV8C-`eV1Er_`aB25Ac@&tjT#ovM4xd0`0`_. [#f1]_ + +Joint trajectory messages allow to specify the time at which a new trajectory should start executing by means of the header timestamp, where zero time (the default) means "start now". + +The arrival of a new trajectory command does not necessarily mean that the controller will completely discard the currently running trajectory and substitute it with the new one. +Rather, the controller will take the useful parts of both and combine them appropriately, yielding a smarter trajectory replacement strategy. + +The steps followed by the controller for trajectory replacement are as follows: + + + Get useful parts of the new trajectory: Preserve all waypoints whose time to be reached is in the future, and discard those with times in the past. + If there are no useful parts (ie. all waypoints are in the past) the new trajectory is rejected and the current one continues execution without changes. + + + Get useful parts of the current trajectory: Preserve the current trajectory up to the start time of the new trajectory, discard the later parts. + + + Combine the useful parts of the current and new trajectories. + +The following examples describe this behavior in detail. + +The first example shows a joint which is in hold position mode (flat grey line labeled *pos hold* in the figure below). +A new trajectory (shown in red) arrives at the current time (now), which contains three waypoints and a start time in the future (*traj start*). +The time at which waypoints should be reached (``time_from_start`` member of ``trajectory_msgs/JointTrajectoryPoint``) is relative to the trajectory start time. + +The controller splices the current hold trajectory at time *traj start* and appends the three waypoints. +Notice that between now and *traj start* the previous position hold is still maintained, as the new trajectory is not supposed to start yet. +After the last waypoint is reached, its position is held until new commands arrive. + +.. image:: new_trajectory.png + :alt: Receiving a new trajectory. + +| + +The controller guarantees that the transition between the current and new trajectories will be smooth. Longer times to reach the first waypoint mean slower transitions. + +The next examples discuss the effect of sending the same trajectory to the controller with different start times. +The scenario is that of a controller executing the trajectory from the previous example (shown in red), +and receiving a new command (shown in green) with a trajectory start time set to either zero (start now), +a future time, or a time in the past. + +.. image:: trajectory_replacement_future.png + :alt: Trajectory start time in the future. + +| + +.. image:: trajectory_replacement_now.png + :alt: Zero trajectory start time (start now). + +| + +Of special interest is the last example, where the new trajectory start time and first waypoint are in the past (before now). +In this case, the first waypoint is discarded and only the second one is realized. + +.. image:: trajectory_replacement_past.png + :alt: Trajectory start time in the past. + +| + +.. [#f1] Adolfo Rodriguez: `Understanding trajectory replacement `_ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_future.png b/joint_trajectory_controller/doc/trajectory_replacement_future.png new file mode 100644 index 0000000000000000000000000000000000000000..582a6acce85995fae84013dbc0ad0569d3c15ab7 GIT binary patch literal 71754 zcmZ_02UJr_)HWPM1ocW00i}yXn!u$A(os+X(tBtsN(;T$fR)}t?+Bq2LJPep9TAWK z3B8E)CcVRVg5LN2|8K4DtmSfY!-g3s$(&-Jz-QAtf$`0mi_R`@EpS_bs^138F2*e74$jfPaq^wSOy2K34oNxBM27eH_ z+|1`aEJ{K}#GlNmpJ82yl0nr~V{j9Dh8UbKFDUYf6Jj2Dzgv$V7_5YD(HWU={b z_xba|Hx1rAl9J;~+MvhJ;qbOE(*IH8>tAx!&H)5AU{~@I&A`@Y# zvf<*h0&{VQ>X|-({(8X%dTZCcx7G$mvCqx_vOZhbW_o@Zu%*t20ixakI>eFZr0LzI zYGji*9jUAiqpDg2cTIX~bXLW^XWB9karug%$KO9!2%}4VNh!o5FUyvQxg3<>hrjyz zpg|!2H3;UvOB= zpb{1ikpSC9SM)#))yp^kwqQLtd|(|Wpr-7lq3Q7`K#jpnR~dTODjOM#gVLJ)9mn6g zpd?SE{U5}R1f=+yYCgYJwFOV@eFbo^A*2w6$vugq{X5*dNc0_y`;rl)4vIBpABrB7 z)3k^9f)(OxaaO*Y=j5V)_Y$pRr(wh7`s z^)CxHpZ)JeBz(vbf^rf!=ti?ml;b20pF@Zqwlko7Yq zDA=k7c?sgmekzBxHv2x?o)C_Sq>!cL+Q^kW`9&&=a*EwXX-(cH^cpM+A305E}lRJ;08A^rv+uNr&vFBrlRSTG;X7g zJz@w1p-OKSM{joaBD7}Y_P`|Cis*7haz})kePSdlBsgE|+5X8vM{sDUix`c+I)ah~ z9fUW^Dk*7hy0p77q})PE;Rf80rN!q(+1(Z`KJdY)4+6=uI@L_9bEjH{ zT5(zVBj2X22|~iSYiz18l`SOTo!)*uY^`8R7jCrp?1T{{!Ybw4Ft)b7JyB_EJyc&q zFKV;s!Unp#JRC%=1X0oHD?ZsJ^7mtu_GL3Od}?ijPrHZ(C6x%h6J*(Y9fKoT#A}pF z)J}~>^up&?sXF?jv^3SeI$nvFL7Z)KPWcvWP2^->3;=N9D%N--V}HfJAOtqvIf0Ry z;sJu^M5iW?w;Q?=SJ6{`YK#aKh(dU+7&#_S2Za8$E9GEsT^?@)=Aj;zzVBgA`aS`d zfy@7e&Riz$HZ>2IhNd3G7R(guqKMUnt%WBAk^X4``ijO9TJSC9BH;J9D&INdl#}jS zC1LhHm{R{236a;gU4ICL>={C|wB;Zx!0IR?8im@Q|2CEUI|>q&*UujD0< zjZK?b2otC)!uV}FoBTJnc%%7XIN}!~=I+wbmJ$?FhMPBlo%34dA#-)awsjGcLhZu) z7W)4+f7%#4YLbR0!Hxdhlhu; zrXUbJE%tpvKvC^>UvqiNnmbYuAL`XMT1*I#0`*eHJ0X1gzNQyle5&k%Rr) zwb|mL6}YpW+s>&-7NuG}EPtO6gFkEe&Y1nmIRN;v3tX0H1`(wQwWMDT+@Q~`Yi=uP z9BDs_eUI581Lu39r^gfSLm6=2fM_DJ*ZVTtp$PZ&&^V-D{V|2gxkoL%u=TVHfS!~u ze0uVDy%hD+x2rem((oQ;&&#)~nPn-DUTyxo?+G^V=Q~>FN$u)%_?OCGz1(UA_Y9JJ5G zBY9m`5hCTpUhoU9>sF~5!RW|M?oSzmNLm)D-2XyEiU5((a#>lKm>s)sxU*gbfyi8d zb>^2s?}ATFMK&1pe)_tJph^B1!9Vh6VV-fGbGdc8P`jjN*l{@TCfptaq_B%imMp&{ z4}+UmJW6D*vak#mnBQnmfH&G%hA!P(a!;E$H-JSPiL?AQ=;PErj0Fl~dfN=nx8w@p zgH$Qjw%sn*$3B3JvD?OLv*w2&SJ)>wxUkM4?$(^5f4f}{Q)0PkjylZ}FmFQ2C|m!! z!@M-&a~OfyV*%XYZ=-iaC7d z3lr#r(1W}39BSTsKm-uvN>Gj^e%z?aSp5w5RX&9|f3tv*^LTcM;@S1!TU93kl* z5C7Z*eIW^s9Be6{;9B~Ci~x7t7E;#~h46rFF}T{)YrukDTwlNAv83@zUSr#5F#_fo zqz?B3J3T0jV$enGZwoRKDg;!ycD!6iyYXS9aC12XK!p6Qm`d&k4tkz(3V)k{w4Z!f zxcf5Qvl7u5C2WiZK^oW<;vP*JVLkfpL+s4Hlc)07=_x@iP(1~oSO0dRyv1#F^aWZc zhh>Kdwhu-P>V3?%?T_Q^()oEa9@(3=M%y>63Q0Xl18CaC^)8UGexBxSeET*CcC+Wi zkoYAdM+tS!WWo4PhaC6)UrZPLybT(>t@mh3d@hV&e_7r_LcZS7p4qy>1LWs#%=@9I z_;MQK=nQO|Ewvx%70T+Py;V=Ijj!(o6pnXanGIFy@NnECA`>|E+ELfnPXHn_xWmh3 z=_i5A6szs82a{3gnZV(j`mZA7|N1xV>6$u_g@KH2rekN0Qi&GsC?}HT5)ETXKkHc@ z>Ir9*Do7A_b?J-nE9&el>oz5p?SWX1YU2Tp1^_|+mo?pUlLld*-!F2_h{#^3(2L?e z2$=1R)O$MBO)U3U1$+u;UcQJ>f3{EoQN^RQUa8AA?=cT^TOXZBvx1r|6C)#!H!G}^ zlvH(BXn6;#Up~nN-a#&6(mmbo_$x(>*FlMw+uE`VBS>y~Mq*=-!agOT^mKJ*xLE4Y z1X!pAj>mw9^D7+%Sv=^tT`s5Dzp?&-!K}FmLozp!A=#b);Epy{6gS#;dgr$63+Ec6 zWd(stUtVrHItE9CIE!ELXT`Gf^)gMq-Jk)8=^0xQrlvh1fSq?MR3YCXY=q4goGRR2 zq%l)87K_j|fbAbu+ZQ1@z;k!Zmi1xVzA4xL+W0Q-V?+@qBupSenZB?&vE+>(OHCTG zP%@dl>$bE0LY%TbmlX!{aKc%vvGp`rSzKSs?{7~4Bibma%W-FNSe8%$K1eE9ihD^K z;)!qqx5d;{CL&iZ28rgjia^4_y%IXqU;j%I`tmVMCAeC!?RJqC59Ao^8U+NXY@b^V zavjn9&1iDGoMyxtCAGHk|X2ph>fmC1K_^kEZW z5+iJi?RQ$Iy0>%I` z9g|{YgjLc6a??DY$tof8`8*d?2~XXGyXjWW`$O$nq;=e>HEzaQ=36E4u ztD${wW8NFcYuf33nU#{bQDcM~Blmh(oz4n2c-+3nQlm;t>zfL%UUshrMPt-XP?FeOgOS~f#ttN zeV2~Tbjw}9sC3DhJ%0wN@^%AgXk!XYOR3)8t#Rh>Sgf}WxUH)^P3(o+TjOv0nJnOP zQeUDbxa_5r>Sf?(|0_Ec4#f68BQ4lPVk$;%Z<9h=z+sz=?knTaiZ?fSHsN>@^Z(49 zp*s@_g{{FMXfL;9B`*VDi6CA*&|~9~l~4gDcB$2o2{TmBZ!KLAQLj#$04PtO*s0O= z8w0`G{?K{YQ&bVAkKRmG$qS;1v>4`HVuG!iDJ%eP!@e6osEjQ~K)uNuX7*b_mvpDn zq|`Q~JpRqGl&Q2c#@G&h>mp!56SLGUTH2g<@810^T{o^3OCXcYSQ%)jV{2}SjZZg~ z#AQnmWJJc14@!dvAkSxVn#zz__K62vU!lh@5(jD_N4nTLK(RB&2~YTG5M8cKZU``lX z^NRuam_)dYI@=LLSQyNX@J2R&ot%uhF-|%muX)#U0%2E*gKO+vzDS4&X2Etzo}~>$ zaqKM~t2lw2a?Hbl^V0u7!s7sXlo7w+3h@Nn8sJ9F5bY4J0U@9;qsI5(zxu*u86bMy z`&O0Wq5XXuH68!e(2%)l*h=F%c8I<2!DngDedD2sFjuiV z{<3=m=f^T&Fb6&g*;Ic@r*iC(v@Dvp@js$!V!@LM&b)7|^V2u?LQy4^TWT0-KzJbA zD4q6^a)NlWZXBybRU>1OMXNSF#`VKMg1IySJS_%DM)m2bv8a#bbpO>b6xMF`!IaIu zA6QR{qFax}hw5o%Bb6Y^A$&FIv8kvh0TdE6q_SkwkG&TOMzj7L+`C{VAsxL7}E>kmR<5fHEba)x_Wl>+m2 zX3t#|@QN3uCK(SmcYPSe0hsG=^UGYO6x7j3J-}a=?n7>{H*hQ|n0-%1nCF)SO7Q^s z>o&v;-0&d3Vd01Z#wzq*FPd}(6b5G-TBCw}38C=*M9qHMUS1O>KnkW&w7v%jfi3pb zWP|^>VV*z0|G|KNGs4!?z{VU_W{E9)5EK}rvxIPKKwia-KJX*?PZYk;u_9=xNK4Y*#40fyME^bpAFE+H0$p! zp%NwZoO4x&iZQT|4u;l_LgRF4^7|JFAW7#Xkllk~ccPTyD;U6Qkuh9LHz8|)oogjK z0+gXvp*7vmV-1zdKt1Oxhp2?1sozdiqtCHz_S%+-j@6*lKBu7g=-by9vylKw1r1L$ z>H3aS5$k0*K3;FYG2SB~Y1^~lq1YW!v+vTdHBopkV7qsnBheg~gZC5_zbD*Xmph}J z`htoL_3mxdm&ZjDmQr@@+O}hnB~^7fqEXKT2nvAr;6^!g4l+usBwMHu(t_>6n}(Vo zqCP@W`?{g86fEtvXK-4t#^QT4Hl?c%RH1dMCJTn2hMjDG{l8M)8aNG)F2Wdc*F|9- z8e*TcC!i1}@q0ciI0X@z^K1!PqSfIWp>rm_2{NWxToe;>yJkANn0hQq3i^}3?rFQ_>vMMQLk~ULsQ^cvh;Y8o{%AMPR)v z%=~olkriTj1v_v+^% zO48>Z1d5&_<*8|UvKir3WGdtPdohcSuxT+Iz_O<$Nbuz910As}+->LRYaydF2yx}3 zTmkb?q#g=m=xc<3ij9!0J1@hf{Wb&gwnNk-_+sR|TJrtl_$E%CdoBF?t3R5WYE3+| zP1i4Q=j|7upyG(QANOdV58_U5W)4Z>x5`4Y19hYx@NV2r9MS_QBS~ozClUc@zzTk3 z;5_A{HFb78^{uaad!->!g@AReg8*~vQ{;}^-y-9KP)iyrNXH6yLA_U-e-gH3dMasn zg!jWlD_+h#*4a_VUXS|m`8^+#1a4!WFbV$7A=gAr{6n$)Ts`YCM?cnFiS;Ud865~vS4%Ng!xRr(gXw#po^psdx}T$KrQMeqIPNA=_;`{02YdVJ^*>h zlPV3PU)tSh90-r)!OX3oF`grWYewbm8|lz#KZY{t>Z59MdAw|vS!E_qoyL)Y}< zE6NaaaJs+SbVn%0cdy7)+U&r#D&MZ<6TBv#pTw?;*6$QkR#xURlp$xW-UJWxyYD|e z?^d@v^~UrWl1ED~fk(TE=nrK!64|VnPkJ4?#A9WgIP)52tH-eg%B!_+JaOu{j&8sS z@JJXUgkOHYeR`7S4Fve~V@732f8G8R8a3r}uf_=LdDg%t$@VxJU!-Bw9S2ukT>ENz z@_5R2>^`J`nhB*g>8rPEW(GTPCc2D%mgcRx(sZnfxMj=%9gt3YN=UOliGMZ}J&}U` zP$AgYcfigTioK^w_YNE=;(xj0$ZNQqe}3|8%5RyghVR9v69AaU<4}W{rPMkenG6Cc)ZrtP`8OM(w^EZa{ zcxvq?w(BP6faql2axU0nQrtB0v~uT_;=Cd2ApZYnFH8!&KElNl=@;Jv!sCr|T6v1|@(CNtAj*F^t%*Rgm_u%>WcOvH#X=wcLkDy8xLx{wJ_=(+ z@aXAR&oqEVE66TG>@6h<4RPL;n`QC-zF;#l&m>WBj=|$zU!-ob+ra)TDC1iJ6sRfa zdsMF{KUAU8(3qQC86{%$`J?KMpei0ia0c1MsG7bgyrzq*XFmEUHEIp z=R6?5W5v@Em7ReS>%ZyijwZc%QmhD=0s0(Bc&CG3<;b<+ehXmXQ?LA^?vwnm?b!8| zSvRje;UTOs7i)4&2a1KC)$}1M72|JG&@jM{*MhBnd_iAqny~)3XybHP2!L*@+8)FQRDGR5;fsD@8p2JTY$^7b)*BXvCAx?L|?WKmq`w9jQke0(Arz~(Kd(&(0;(LXhW`yV8Op&(3- zUX`7N(t1aGYo1D>u4)^R`csv-3c3(N@-2EUFox0M2*85Kt~AuSYI%5g=%U*v-! zz$6*CamkmDy=Fh%07IgpqH1^B8E9zkECE|1nlwtIi9*DUz^l>rBrPo$7IRN-;PRtC zd$zv-mgnLuz|()2|5SXW`BZ10hH{>v-jW-OTP`ksCeH=T@V0Z#tuD1!owq%q5rK=n zqgsh`b7iCOw^md$HC0#tVPUL^7_GF<&QVDcOC|F!(8oTsUzaUQa$fxXG8>t*jGb&q z4Zp;yxGIz6=60M|QQN>jcTw@N_NxEM5zF-J{ zEdcNif6B*@3tyq*0G1f^`10S5^=v7GePHX!k8u^ABO~9$ytMChgfFf3XMOAL($JUx z(9FhmYuz&}JWMAGhtkQFd!KZYMR=sFIuOtx)=nTssKs4=b6fQ>OXLiq3}b6O%QZ#q zb&u93zIAoxZ7mGto0^7=R%p!PP($_J?wIv4y&Hy=JKgCQS@RtY!CmBvfG|qzlhMoi za53=> zef^Y4D~l3$T#lJHEKD=5D|i;qr`kdE4*CYXHldZnX*K_RO~3NF1$F=?+`UvGY?~%y zn!M1br)^VI8+!Zmx>+y>}~C>mhBZ*1#}FC zJn1)B#ZFHS7chKW5X3Dl(We9E&$VoP38e+uk2xJYaLtjgqla`09U}e@b5W~C*5Jr(S4MH|HPRcJghd?( z$8$<%dNma}yWshFJx^VIg{=^KXk$YZtnZnolM?~d?<6KB9Z#L(4i+wvnE+*4Zo;9D zZJQk0?2|cM;zjzE+(ZIr!4M2sEM+sPxpjEM|0eDyHv~ zg#@bm1dv-#Hq<4#ujG7al^;@5RM+?L$WGtjD-YQ}m1AaMV%6z}Po0=E;T7T1&j}=R4kv zh>cFnf4_pW+OL|jJ}!InZC4-~zP~j$TIYrFQcK7nsv6H+^xEy^x!B~sOn>o=FNbjz zytVFnKNtI#gW{~wSbc3uo<)3EAE&C0qSob%0^WKkTGkZ07Itm*C z{el`pn$7$69 z6z#;EkVmJ}q``H+5ToJC*#_}^weWAZ)()0=e77gP#bcA8>z>c{SNr{#{fzV`z0pS| ze!jf9gf!|uDt9rn0woB;9GBJD&IbA=g=9v%?HJDOyOJyYg-c-xZ?p{pNN$k0kfEw@AXY)SWM|OO}w%dGMy_at<{wCLROqLq)|OvHKIb;ZHt^ zMV2i$-@E6!)FWCrmLLE<`${@0=+_7kSiEOf2;Is-qc8pO$#wz0BA*ySz=I6)->(5! zyYziBjA3`HHAq@{G$G;kjrv{PiLC`0(;cua#Far57x;{2NX5R%cqXZW;A5erEJY5s zhJ;Cx$rIA{<;VsG(dD_T)G?k%75LIj#KX_N-V+Dy4D=o=D55%fS)Y#Zqq9PE!Bm)N zoVVn5>(w5gEeN19HS@-P5^Wxhh^yU+VOge&PpwNo#gvyauh$Oe{=w&- zWRdMIj#z`<+6KaWD9H#ms}5Y#$0#+`t&7jSe&fj$5+05VTtVX_CgFyqG~XZIxKCK) z)~@Iwv(;gz2$r!ry>yw=in4A4dO}_T@wpLR$_QBr-;Fg%6w{~~88PX|%*qm3FRt5p z&!u&XhI&?3+~UJCdCP-)5sL)@EV@8G!f)=RN{Fl^TEuAeu9^|XsZQ`bzfKCW4lE2d zlP!@bvrl&`GRilG;jQ##t^l6~^Bd;KOv3B7oSYkqBY;V( zs7ZdNk~vN!Ug9TjXxx_)vBImGO75rN{~4|L^}6S9j$h;~JH8&j;`;wNtt1Ep6D9}H z;TC($lKPUnXuId3&?=jy*X$X(1Swk{Z;FezZ$iC0InnG0U!Ou_oEMZ3^sOB{vj>iI z@sFNyKd{>HnWHg=ck^Nphp%vDdi*LLOW+=#Wv;mtDZ^!bB~3)5$ND4V{p2Z5Nx?FY zdUt&BHRl@bX*aqEu-l?KERpDPdRkD>wd>clZM?!2|f1gl%^Iy*NWC`^zalLE|PF;p82PM<~SxwYDtJ3>u0|Yi!KO$mpv@vIAQS zN-D|1j^b46JeuySKgwGI;*VuZ3W9@z`uolg)m%SoKfu3rT@gOkYtn?P0@G1#`DFE- zAAVPFxF%hVMchZ9D?h)s!hK^j^eSuM+W&)By?)B;52XRNE|k6ZbC3$1oSl7mZwe_F zIk2s^OGR3uNC*NKfIB_1S}pJ6u1_HiC2)lh6JH(f&6XvAENWmc{Vt@vdX#h$x``JcBCmmQXh_Ut! z0@D#;=jV7P#G%$Wef&C28pS^aAEO9*XWDW1iSTj|M=(D40oPIpf5l9BAEV+0L5j@+ zXjaB2#^EvrAVA>Xfej<4r5s4Zj_Y{FJgizm1067?tF|JsJrlUj50Q&G2-U z_nNX2qAuf${r+{giH^{I9SZ415tGPtG{32j#+{`lawH)X{e%B>IWZVCfAf0rM>rF_ z&XjXe1YUnM_QiawUk z)N=de@Xu^O_J0Q7^F2OVGqD9Ssl0Z~)HvRq;i8Pim;fz}2dV2}j;?b0$_>UD5I z=LS<2Yh6WthD%f|s`=Gs9LviqSPf3NA9Lv}EE?cQ39va4#($%UQS z`xRd0Z+f72Tc+iTy047inI}qFso)-DL(SK8KsqUB_^4F11F#0sC%-UIo=7?E1Ivpu zw)J3Tmxs{?9AtL*ap#CvuU_#jGUH`Vg;xfSAkqCgL!-M#bm>}d%h38i#O36 z!=d_)nE~}dF9)-P)k9zS3<`+F1Q>_D=8 z8%~fHNz-ny{$E|+?e+;-D zQ(lC_i=n2o>{L@f<9Zf+b*Y!zO3s0K4|l`ET07&3qi;N3C$kYld7j%zp+SOWeJTA2 zya{mNgy6E2tu3t}(C>079$m{e)3cun_dN`Iw|%V{OEpS(jUcDc$`*kkK;Y@M(YA`W?hC zw|MDkC#ykfHO_T}@-6o+@Umg%F(v|Vwxkq9^v;4(4ZOkh+wbZ2o!Bo4Swdp<)k`B8%K z_(4hnKRfxlhHD0L$L8sA133Xs_|Ci37|A{s%8g+2VPGdWct&i5#v8~|65NkBZ^5omrZpI5eJE*>vL%S@Bq zuO@j{Pk*V2d(QmL(#p~RjGGBylP~XB2)c;vmXPN%>&}ln&z%K1fE0k7G*Ax1YYtGX z0F@5cM-*#BeCE&G*RWd@(!c4^bS(Va1L@+CWJCRRFujklT;|jgE}Z-0=0QQrQjVGU zgYazSZr){}Bu?M}JljuF!-JTZl!!CLaVY%-mk*lK=lBJ$>3PzfFFh2NBh7CBJvEXP%6YV@Zwp!o0R13i!f>=l^OO zbkFyQcnz2QJZ%kU5^?H`yw=xrf-}^qax_D4z5y!80L`KAB4aHP4vD~5N8JnAA9J@u zzsw3atiI$w5YGIlpsP<6XPunG`>4Z+>r~Siv#a55A4LWjxu`)JNK+(AI7ZY%39PC%Zjj zZK5zt$Rh6ehXCmFLh14Pz>O&uzlVp~N)u=C$7d!))Agp(JIdL)rSp>hAryqwr(tF; zmODL`U?R?|BwoS8PA$?s!$bk{r#J2zPaNcM7g}Zc8OICNUK%gXdb(7{-}nO{!pdRj zlNHk;&jW|H_O>B;c@R;KYd0;)Zmr&$H;I4wmeW>BxO(vno?Nd=dV(EdJBo1>y8+_3 zfP~o2N8Y+2@N@63H9iq$W~$GH>2T;wR1`V*3Cf|yWvGS1TT1@{hmh7({eIe$&p^PH zSoPO_Ji4)(3nxD2YBZ=Ha0H3@6QWnC=-~I0RLK6>ZguxY^%E6t4@#PM%77qQeq@(| z_t^T`rUX%P`s4L4TCjjXJ#cv4L2jEO(haNE*_d2*pA92tf2Vb2!ZDx{V8lhy9Liz* zzS}AjCwpTiLUu$T2W$rmya+h?g|Qw`YC$sa%v=|sN5POFM-xQN|ayWXaS z#nw`41hNz#p=0E8*nP!TzdSUBzkuh4F7v8#e6*zIzeP;CyJMT|SlBGwmcP zQ4T#J>6E*WZ{NQ^U_L*t^NVL!P*7M3s5)0i#Ka+TK04DXA+FqDLaI)PTW!*X?C$rN-X>t69#WxB?ErI%gxbIellarISXk~HIePNwGxRh&8 zE9~2;Jv(GY4+R|1Z)O1juH)poj;q1S(D_eah;kA@eG)R76Wy&C`iS75{ydKKUoQpj z-mf*0y3x48ck2EvMst=B&N&9{7XP1YwMFfl5HPl&cl}BUT7bZ0q z7*(Z;VolM5MR&?~zGzWc{wR^7ndIjMtDqZxC%M^7XdHdnY&K)iwf|vpe(-!>(kY^t z(eX{b7WukF-pBH5A$=d_uhL7wU24(=yxVP1rWg~%V5RREu(9RJgR`2Ku~0(THLlDD zpWlm#jE>gwQGqC!mF~U@2^qa-^(?r98p>|DQQY8Vu_e)lC(NaX;o*wf`C&Ahl=2JI znK@=Mb*nf$n`Qeblwi}{7X=Sd6G+eWo&n>C@ob*rt+R4llct&j=j8js*?A<7DYayP z-56PH$|LcVlvNS<;aXK8y!@L*D(is@i_p`nK|v<7JZDFGZ_>nO2pH=l;k#U6;`38s zZK6hxDvD}(5EcC?oW3B=7D}7$3@n(5@172r!}GiW&`r>E&*{XUh=;YR6?{xC`HknZ z8{I}`gPE=~?{|7r#Ix{z+qLt3DW=OuMDIy&HSu%5>i*W#Gc+Hzz&u{#*9T9PgqMvw z<9+IK4o<(^CH6ht4EY!kQ0Se$zC4***L<%zq$8xgc5fNF?&|Hf-k*MUz=7WU+CoYf zU$LTFH1Nw*g^$lFS=ctSu((l)+D}-+?0%?5diN3M_w8g+j_~v!F$=ki(~a+SzTIH0 z_%{m(R;Lsxx90>{!+Q`SRO;nB0A4hHOX{yi2e8D|wYJO=$PIzJJ(b=|cs~OW8(r<< zJ%(X5cNwRuo6!5WSjNhoBhtL+jZW37-*Bsot>Ei|x!D0{KY>)j0kNJes~N4dH_SdKcYU?I_)(^j6;gYA z?7q^v+00Tx8>WNIb72;A3Va&QkRKRGhB9uf^r*DAvn$R~ODsM)@qu7JBy%jrdE0aG znTIfryQ@d1rxp==lXtGU!S*`JMV2-pI^W1yUH&YUkCbn%lc1myMXeG2S$TRXtyLfl zC|K5-dZX|MaU|Dc0DVCW>?r+4K?d%7oqX(2j;%lp7Mn?&uAakn>zjO*oY1g%UZWf- zi9JO`62>wo*KfZ<52#!cOjRZ0Qpe6EBV#HgouJ|K08qxhq_BE>}CtSK4s`8%1 zHT;2ApSlG^k&txPIoNs z!6yC2Be$ZPwQ=V#M>1Z$anIc4LA0sYk2Rd$M!nVqr9W49VeQZZRT9p)T$GaKcNF;}CS0SGV?Im_9s^%-u=PJgMcy$&3HHIFkD zxk}hD}0JaR>c}$YW%Syb48vmzEE=} z6whXmXfWE~TgBc5e5a&$ZXzwiA<1TB?Bx=rUG4G}aL$kjW3RUCHX)S|^FG=~u0ORm zizX)H_jIdFfGnHOmETsb6LJfsHU@mc2;Tc`B04z*9Lg9fbM3ws;l4DfP&MI~oaQle z75#?(+qWJmy>}LG0Q&g5ogR3_o^P%zul%Im1PclZfg!-ree|(?;X&?zKQ+#0CrXuZ z2VUJ0yFXb7lzIVf>*xKeW})J&Zi5{H?5OVcS6Bsy1?BJgdWACNZp`-%MG4Ay90BB( zQ1AU1Yv4^G``hbh8y&{_^hg}BbC$fZk{U6oF)>^V$h}|@+w}(EQ>PoV^t9-aq-bZm z)L|ep@-i}HHD7){-V9*TJ2F@b=%XnFvn1#=z4Qg|Js*q%=tI(Ah39}K1xsNtkjk@H zmHWT?lEyk^{C4e#V-k0nR^NOC#@CYivk}bR^ZjspHUv0in$+O%8NT(NC2!sH7Q3v9 zSRx`*gvIxl$Yg=Ly(^Q{=v(>URlH+W05~FC#P2=XK07v@5VJGFY|J^;2p_E1?H&%O z#na0;PWwOfJIwY<>yRS4oR$ix)1f2hruI}v@h8_U_y<2Ed;VtUV-7o$@!n^mor|p` zI!U|p?S~vC<&ut^b_r{sf4k+NViDw_t;)lO1?90pm5H7lI#Ih&uH;It`}g-~+pe%F zQmtxc4}+F$zwHO`IQl%x{>Rxo;n>}{2Io1SDkRD-AxL47Ce*oGw%?u6G&7<;^<#IT z68EskpfTrJ-`F@(V#9B|)lsZ^gSCR5cW-SX;6v9{(An_ImAP3GHG8$;;ZYZjTmKINslqoUx7>6I;v!|7_L zN(TM-A}bdZ=#uSwaCb1Z)_LTYuC5|{@vdA@=Z|mP-0T=LFu1{_kN+VpytGs-Ejml9 z@8}5jPqH|TZe@Jh>@#`dCy8llFK@AM(MEn)#PW%X?mF?M=oMCIy^8F}V3s=jnL1}i z-Tt!>Y)T|18eLhaaoluRvG9ziy0)fSW|(EJZH}|^&`fphu#{9u0{k@m&PWS7$DPlH z-c-rXoNw}MY|QIf`tZouSD>**Rwg9_OUMF?PovDMoBbfd>rheoXN^@tl59bg6O@QA*KM)`Ftp?sigIkIS(xronZg>4i%NglvvG)qr*Jq)LoC2fvTg5 zw1{*HpA#Q-*ZP7L+0tNaaPgB4KAXmHMxmR)?DC|vLJDSe99UMvo5vr6WWOKW);BcV z=L-1@yfe-U>qW8KKK!9HFfdrQvxM@WJ^Jw>PBgl3!;A453uTnFkbCMLJtw|g$#jCyp3kv{l$%q@u2LI+E~t|`ASh>9vI@%Qho#Up10E0RHL*K|B% zx`*9sxt=9@?HJt**8TBtgT&{bs08lS7T1+n873G8o~)h}w^a9{UtZHatu82pNLY_^ z)^CqU=D zRrOrIg6ta41O>NiMFs0h-Nt1w8ynlQ1mIuQHFZQ{?y|VIC&YR_-z`{tK8dFLQJeCe z-OTP#0`EtpidC-kc^Y^c*S#iwM?Af{Jnm{bxR_V`r2d-IwMY!lbx=l$ChJ4#O1vpG z$mkWjJ=*9%D$%doXPBG+w0!pP{SVM0Q?#snE5Vos72)&duUD*cNJ3d6GOkizW@mH$ zCHb1{sS$8i$iySa^b#c%Rbog`G>6;iVL64&`vNW3E|_w7+>2B6N~y&HbJ4MDs_N`+ zePCx`u@~S#9zZa5cTURw2JT9Y70igY&D>qM6**+P4@PnT~cu9z8l2z%Xl zV&hg!;P;{7WcyEHsi~?eD9yXuA^d+TsYf#?L#lFzyONKAUH5x{v85>ZDaN-Cqz-=XQV@ae zj0rk;oWEo@i4FYnL$;Ik$!7m68>5=imDv+%xODaGR|&qF`Qae5Qh=fr=M*<9z3tS9 zLaYD*9K_Oi@Oh>3`TdsaR!85nB%_J*;lrbE)aWlVoh;TEdVBbmIllVidXk{?HR~bdZBl3{BGN}m!{yL<7n6?{<7$p_0raC z!Zky8`1ijJ<;G6}0-AfjlB0T1z$Y%hUsO}LZEWL(4wJeRrx4-2y{dWQI>r*lg**i_ z42xq~81wI`Z;3rgBTIRSp(xt1hjuNMVPow0Lz1JJQaY9v;MGdJ4y{28Pi3ArxLKz+ zF|*N&4d($d7ou&b`{{l1L&$R0eDBAsfPl1Tl*Wk|&7e)usnCiDX>5MHg+&${KSg8K zv+GJv<>lkdJ$4)_Dk~q~uiNS}xZCph#fBvXsHCzK=iSrFMNLIR{c%VJ^>nY;Mmt8w zWny7^5i})h`aMB{n3_ZF!*lrILWR!gt0UKeI(lSujw;qW_3`H~LwJ)gvw(0S5F!_B zx(Qnf2QcUNs!Y1V=#lU(Ccfs8+HwLQ=H%Q5`DaC&;mN12KVvZZHp?#){(UV`e{+f^ zj}s+p2xEBdbf@08p6WJzU^T?icq;yC7qz<2&N$vTbL)d#crm8DhS zYr=6>PO2NXqJ%8iHsVbkD$6u;(}4TM=Zp6;80-^*0t1hh6GhXO_AvWWo~BjB&aTdF zyzW!)4A&b;fGDsO8v|vh-VE;zm+Gxre5)`~yIy&0x3W1pLB+%nwm59ruut;o%FkE| zlcjzyM~GvfaxgJyaA@%BZi+MyR5nQsFEf&gG{%jf2lwKC5^ zO7G<)znJ||rS&1`*EN#+<9AcL-IqJ57{^MV$O4eb)>2-Ic^eS_44qU#oSIyhJb8aD zwxXiK<#gn0luYzZ1GWpl{keLyqXp?1>Av2dpMMGaL-v-q)y;Y~Huha9Q$JL>;itgB zz{UDcwV#S*BxfqF*p4l7wT=G+%RWJnrp_oDI?r-W9 zzIFb>^78VpzkO5wrjcMc>^cS{fEV;^H$QGD5~9U&(0cXk#2Mo2V9D9cp+*}Sc%|%f z*3*}Zol$w?Rcg6uyK7ZsDoQ_#0H9-Q7TTA3e^l$0BJNmFNPCm&5WlELDxhb&Xtgm*zgzCZ{sEaxqv?aG)h$VNr#I-su0MX8?5`8TQh*qyky&=)SyUt6phc< z4ExsG_sk#7?^7@49u)eduzxyPVqMl(CstL;1W|ZPwY;2yS<0a}>W_N{} zi*f1a8|UiA`gvO0WaRB(%5e^Qy!{{zpUkp3%h{{P!&loqjqd`6F5e3xm0WMA?!1GM zf31zv)eU43`lJFCEVyS_rIb_w`gd=02=-LM3(Z{++Gjbs*nc}eo!qbHG}EdY~zl9{RA z;Mb%owUTU4nG(s$Ndp)t8t}AV77I^Mx*_3@pZ+^&&)J;StkA*AcF!y;E->&)MJAJk zx}FJNzOyLrov{N$`!;*O(?Zp?ol-N-u1;BXpZW}K!1KukE8k4N$4mPcXo1DA-RU1d z%Y->ncLasY$v^qugAhH5H)#$ZN z_y3{ltD~xXqIMBQ1f;v+5C#a+-G}a$Mgi&W7O6uwh?F!)BaO5YD&5`P-Eimd`@VJW zT}%FgcV^z1Jv*M~*{`kFWk37rOORgk>eBKeViOa)g1l+)lX|5mS05gs?ACH!O5zh@ z-@k9(UGDtgtyYzc%3^23^|QDCtOC46A@6)nliiG8lYdhy)R?g&JJ{$?_^T`UQ~1e)n=ac4pADT`9SN!6HDmlpmjlVurD@uEAKGL1i<9>FW;6*sqdyZ$Hz^ zVMPbYDSNnZ*5u`9mo({hA0vlTVk!zWS~w;#6GjKYp1*fJ)Bj!irg317e$6@XA7x4v z6be`~Shrc_(Gi)x$GIMREg-aQq~w2l*Ur*&pfTd*3VEF~E)~l<@PBa^&XPVLoT~pnz;O6!n&oa@J+o4k5N~^`;C?jnTiwMu%Ru+@YFP^KuYU zQU=h{qsl<@Rt74J^yIY>%*-s>!EFvSMhTA!4vUJ)N$=9DhJMVg`$+~S)2mG(O{s7{ z{pc6cJ)>JG%@_Qz&G17d?mhhedNr^%MMqo7eJP-Tn;pTKoojg@Pd@glfX9(4t_P&N zs;VA0bvZ6Mb?<~@9c_O}_NTlkq@1io>aDIu#KBg!-SE_V1?-XvB{@4Rr3zsphYnI{ z2v6(>KUMh7@;cSjY>im$?6O$#&hJElo2s*i3*pj*Wf#g3~+%wxI&gZ?Yay z`~F6ie)Ce0qHk^vn4@zB(2v2su!^hn8&n`pg<*7pB<;8}8Q6pq&qJ*;$>+U8^c@wI z_Egye!vdIgHX$Pd6YoK3^LNzWjV>rMz~KbB6w>cdNr{0u+JX6FI}<((K8C7U7Nng!R5)4R0P#991|_sd7Z?3g3Toxc6OXB(yXmLy1cNkW6U4sx%&JtkL>k$X~GD9NBaUX z0X-FZ%;E$$x8cR}P0}mxmb=$H+)>~-kl`16L=zll%wTte7EJ?=v*`;A>nYko$-dNA zROre0$?2Np-W+Wb{b^{)y60V0Yg5`}la=C1V2urA@Ga@af^SJSF$KYx*P=CFE{<66 zb8P(JfA0qS(#$N)1`4$p5}swSf+tuBl@}MI^+vv+qd&*PU1LrC`4fxRJT@o&{J0;O z+u$lRC{V;_kXhrisQ;5(YU~pZ^L@7qjE8;idYI&LIF%=#oSX~+0W}bjt2g;^Zb`|4 znsFq?2C*krVk%?uwzLzA5GAFYg49L>`Obb+p$AD;9@WP-MCp>eoF}W3lkjFKka&e3)c4URj~)?#J{I6wIQD zA?DT53JC@Hbea)nL8HFc4$BO$uVAF6IyDOuh665{Hv&RBHZ}+x66zv;3IeW|4i{16 z3?PVZ9{&@=eBbtSTN`6sbFz!BQ-q$YcO0LU@nH)G9^S69rba?VH+6%^xD(tDXUyjT z=Ld_3L(%o9N9U5!EiD^Fns`49{c@B&`ClM&{Qhl+i9rw_3OhkTJs7X6|9ZELO8oTU zJSn-aKEYuBf+TX=+agHmocHo_{a%~3KdHOAlG&xWtcgrn3@GdbXL(@6lFt~qZCHv2U+0`_c-E2y8C^#$=f$4V|nOKHx zO#YxdZ67TfoE)R40#B{h8^^f_!q;ca{Q%MYYrk{eMkvw9U)$P7%h)gj7dqV3*y6Q= z&kgogWMrwfrz$Q+u|brwCe}(WQg^7Lb&)eSG#z%-GANv=qF|cjFt$JLVQcFpsf=k5nkE zK#1tObYra zLSGd!#tM5%ir<1tAHHtelbg#=a(Swj-1dm#|9MFEa$+#BpsT>zog-6aq={Z7klkBE zko;v!G#YY@J5aq+Z0eJTLf8ItAMTPhHVGp)AB^6+4nC5FK2A=_phWD7N*Xy%e2C%Q zH_lbA3ma|O6V2v)GLebGxhl@HrUmr7ggVWwSy2R;)!Eh4QG;e2ZYw_|%dPKgCxu?i%mL911~`&`<2_N;#m!ZOVRpXw)fe zZppx_Q2U$l-KU{{a%oc`{>aKrzCuAzaTFPGC_*`(XIYlSTu=py(fcCyjB$KfH9h5s z+r=4%zDLY!fd`zU-PVS3n#bPJD)50Q zCnwuwG;Cbs7zkvkWaTT@bmgw)gydl-?|yipH)~y?tEIi}A^Hp;1!J3j=$gx{0c`ee zVfRb5p+;23cFFwQmg-pNm<0b}#*6Z^%WBQ$FCaO7@=(C|%$B#^dc_eSXxPq$i;o*% z^5v=+zGij4>ZEw0@CCt=&Ad3qoX2VBohJAjlCNLOeYQ3JrA!{jHHs5A89w5?Tc+f# zjy-W2(Nu5y-6g5?8uC|NS3mOK%uMNYZ$$6JPY*T~TPgf6p(m5oCWOuJZjg*wCrwO$ z>u)U4TdR8FhW6uS*X0p)Sw>}x4L=H$dlYyxBc$QL@X{DnEv>M93~{sntw4fae8itA zndju}RsELWCT>3DlWT6El98+dJDu=Cfr|aD67b~CTag&?8$2&xf%y8!+>Gj_fRC;U zpNcLcmGpifJstJ(kNyF-Ej7)DSh$`Mb2A`@7zdJG?=lAQ;IJpOnSaf>r$3qOGyCf@ zjT5`t@kBC3mPb`~B{jA{2a_T3Bk>Dr%Y_U=Si=p2RgL5OaaG$cNb2y92&dKi5 z2=12rsl3~Ioa=$!nQJ#=?~Y3x0XLUKo07)I67~sg_}CAAA|Ak&27%TJWGI56k!@5rF&ph^yL!x;B%=u8!939Z^oXAZ zK7;1}(o2ee9nR=p{O&{rzR*~EVDsHi&P4xtUl=zoc9xmzzgI6vw- zQ66S-z0v*k}cM(UI;HAoa_^)U59Zf0)XWRMR!OW&=Ec z8-s!o#NC+9!-I2SEoH5KiX0@BIMs@`Chwy%Y;9&{e9TAmrHV$gcA&(G>F+{wNfX=W z#wdD}d!#~SL>$yuMSqH2onC_c=HNebaJ0I1F=PHpv%a&6wBIcSoVlFpLI8>#89$MH zzM|`B6$$!TnU;T@c-SuhX0l?ZZ7FyqT;jWE?+2v)+taNeO|e*Y;IT@msk0bUZ}OO{ zpf*}u-wR&m=<+?C4lpv*y zbA$r3d`kh5prwxc%kp82QQG_d>U4nU;FmoF;^?%oLOpYS3mBtyWG7zNEN?f@+OJiu z^^d#nj=Bxrb{i>`W!zV%82Ias)z;4Vrk!j4BNM{iD;2P<)EH4)Xe6Is+@o_fy6EU& zx%}PZb{U*hQEkUC#XEHs9|o{JWO%6^BW=#_j5>;pA+@y#j$#v^$LQ&q*Kve`QB7_Q z0f=hUj67O~tPqHO^Pd~kc@tmch_figjVesr;G1HyG4-)e&plFV9zdX{Y-@eOe@@~B zO}-HG;g-#wkfh(9P3jBW_3$Vz9gcK0+9Zv}FA7cH`^#T*X6AfrVJyMZ!o!zZ5-ZrmERn)EV)348X{)MFYZ+9BJ;Kjg>8GL~0 z=vU3OzxKU4LwNg=3Smz~E zC?!N*E^nvUrUs{GUf2I}j%J{6yLuE(!u5yBeXrq)DL&&>R}0ro<8)Mo%Dcr`@%<&m zxVs!xOgqfhFh_%5;0Frau*OPpYi(n*xUva0-~cl2=V|?Utk9~{cxkeJwW6q@ro%v63X>kK`P-tf>C|a?T<7sqi zq$~i|;N%Vf%UB6(b&*FFGO~*lL@#M^v12OCtF?dDo(dETmFw@{a-x#X5V(>O{9+W= zktGeqAj~{@_ew!M<8xFCCP=%n$S9tZ>X7=cd8~G7zGZSEo6|X^K>drx>{*i0NIBQE ztd|4L)ji{t)RCMa@nx`yXu(HPM|zjuNY=7{Pabg^VP<9~?5N6CHD%N1j-WKI8?3HX zRQ`Hf*PqYOPI$b{I3y;-&e3lBOgktVD+WQwQm4wbX1VKxGRB_!vFx7K=eh&#>@_L~ zE9ZlUF&;@sT9&h~$mx@KGq`#gPW0%!Rd^&m5jYsl644_u%n%fDs4+H8QT4 z1`3m#tX>Ic`0%kDlF4WuOG4;gBS|Yu!_dO~BNE=HOcMU6t(IImfZjCkyCSq6eqENj zm3&RneD~cZPV!r^&yufb^1DWv}oyVC~jyI4J>>jN3!di|M zCMG7~dDY1M2nomB#KqGZ^69+uJDsliP@T?M#ALaTE5<6r5{eyFgTq%Q*1*%xWH<@g zgf*gXpOtl#UHkktPZ~x*P|rt=KnJH}C2S-{!^_9p*iEs-;0#dQb{cj@4lbMXnYBxJ7*Lra4l%dh=WP7Kjb$jw-@wx zT_&t_S0vTI6rbkqN93UVwC4(d6{^Mnf`FH4f@5@WDwHOL%jnn6BuECZPbm|$1IIak z2AwQ9HJ#6=xG}Fqq?@j&ec+km{qTNA^dG4JCSRz_!um{x{l&<8z;(9u-p)Mpuc@gi zovn$8h!`l~(0q4_X-#fDp!kDTFf|u z)NsxFlh*|D`o_`gAeP`il)lf=&@2JQy)!kITUGsAX)iwKp2vCR*;dhaCkxi(=b5iw zeLx)igojUokt~^M}Y^rW~}9Q?_8vfZ@BH%Q3$ z!Zbh#E0VaZua&grr(bvn@rIhK@5<-=uywk9zI(Ls2zSz$3tb53s_mA&0)Rhhw|k_c z^Qh?POxQoog#8H~9vTV?=@x?u*saOwdrEc{%zBx3#m=|w?0`8UN@;y9U& z_w-eiKoH2tp5$iv?YuL8^}48tV1{v}LIPtj0AGA-rW?dBJge%73*z3LR; zD4n*P7hHAu>9zc~{KulM3xpLd!>x6BE-VagwugXzKsJmAc!9_3MmJCyW(8+$YVD--)GCmafScRU4|7iQJkWX0UDs#|lZ z>5QP+VAt&G?9{OXkKt^M9I|TT@e%r(9SLz|!!>O?tl`kam<7 zHldW2`l|0sZe)D9dXiEl@OHxfZF=(A^R@6)e>%Rzg@GP1kFsfdD`X5c%GNm53}~?d zBjgqW%EplEvU*B*jOA%So<6hfsJWevHu#DeQFDzPm&WgjBb0j|kTK+ohycBSQn^D} zV`<`)r`0?ICGQt+o>m#Xh+O=tS=u&yoZi{-gCa<`b9NVMtX~HQ+JpHlq%k980{d-$ zV#7rTr#DW$o&T!^*z6Y!drLtpF~X2+;1f~|zAsY9gFz84lD)slQCwRk-e;eG zZCK;o;V;Jp)+46_Dmwix4zzX(#Ca(SG=&kEax%Xs$ zDhg-$i9IFG3s7vUOi9#pXf7krzKOj@qqsQ~!5hchZtCo9L;um$+d1v)Lpo_|LUUbW z_4CID`9BISQX32b(w5;A`Zot%dsDNLE;O6V(+cRRpoK;SSE~S| zajR(|i(;RA;=oOr7R8C3c9p_D{OCzr(jElB(sXRGlP@2}YV z*taPY>{#mMA_qmX4>zi#mXh*)MFzXXFAfhW4_m{Lu~rF@KBd`B=YmhYl7}4Dn8?2qm%itu-K9Fs?`>ZX4#FtX7K~{ zfw|Td>qzy8^cGim{UdundqfnZB)@D~z<(d0*-xOwMB z;KXIWSk5KY7o93H=RRnAQsBN&%ZaKX_O0=N34>+fGdwuSQf{CbZocdn;UT2r>v^+FWGo~l4Eg)9q`XrgF|!Gxviy`+I2K?Ev!t&Nw_OOvKSTzzd()IJ>TaeT<@<4 zcjCbXyXuFHsF6*Si{!fiWGt26i%1_E8xW`HjQzLA850Q!CddeVrF|FUwfCRX9(P@w zh&xKM3I4s1Q^6!B3nh+WdqSIa{K3np+->p0&+;oIytG%bGcAV;5~*g~el5Khq^a== z0@~Ldqm)@YPRW^BE`P^9mq>XCqU_HeQhe;}W8ZSb#nubwgR}k`#RmI6%>c1!DK&`V z?31o~ceOU`u=D7&wnJPTPW2TOm30BC4~c%uH~sy457J7c8Fg7>EV|WIu+UNG#zm{g zoMkb6YPso`9{uCo(;gZszEqojKFNAA0?UgVe&=f~!2wjaLXEpcGr_@0LgAr5@Mrbe zmhGO-y*mtw_HC(_uf8}A_U&Yw=Qlc8r-w9P0oylb2V6d{&e&JL6DsSyd5wWGGByDN zX@a!Cmk8}GQ#y~%QZlC4y{9B*S*ogZ)j$K8S&}ItOp@i7yK-SKy*rU<7loH~7lwBmmxx13is47(ftZ9oFhY%%Pk(+S z@SdAjk=!^Q3uOipXo~BOs^9Z0gYPwKFvS@~4}UAwDOB!KHRKuju?*Nm5%gn`Y%tz zl5!vc^7u18N=V6cfmK%Q395vKAE20jGdFWXUp4T-;+GmiYt!()c!1&)T{7JUl1i&) zoEW<0_a6X;sYbWC8LJKOspy_dO&n!k(A=n_!qEEj+xze*Xhm8IG=TC2;i7(zVVE_#+F9(o`v~yU(`A^<_fH`ZeYQ%j{?|V3mX(uo($QAlCwP)5Zv3Vd0E0e6N6UzJbMC%EADA8vYGOM~LO^;bDpt z_pv1(A5f4ms;-0+E4|yKP#w@Vh|1cXMgq%}b*((4ts$SKFJunadX_8)XeeSC%^#fa ze1Dv!z1c6 zrZkb3QutJVgBnW#h@IcJ_xKADzp}&p<;UG1f6^zk03H+o2|RX;GT_6q^QzQpU&c{K zADf4OE0g)*1qKCFUOEIM9L7B)QzYekt-R9Sdj<97$`jyv+_uTs1g@95V}wwL#9qVC zYW9f9ge}u4uK*#P;+-ycn3s{hohq?~IO$ruXFd$^WcuGSF|KpYl5!jq9Gl?p2=3u>Rz&L5i_2{7VU3x|c%?wXNoO>a3{ zXrM9xBtQ|64nfDgQvp<){eGtGmtot$G^p=9I73EZ~rhD=ZkaOjw!i-ETcUtgu#AQ;E;cU zF#_t@_qr^@H}e0fp(CLZN-;^nePbGFUxy}%focqFwCwp-oZ`~|Q$p99+|ThktjXl;ixE+6_y;)^enwWSdBW|p(?{=chnChzcKc)0Vg9Dwm*?-C6)PXsmz0G`VeB8{} zWG}Y=w20Dq0-c;cus|j6#cH70KvM?6%qLOY99d!I zDGjZYQG(!DgKX99!yjCa{p+{8V#Z0bLTTbqcC5qvXF!-Cr2QuJl-&ZDZMV})JzcC~ z)0>Vv;O>A7OZMA7Rt1ZXksItwULB8r#>(B+%BfE3TsP*?iYu-pY0r!2qX>TGo&G--*?fmZi4JZCZU1Ju}TI?`IK z@w;CBn%Oq&!q8_bsL!2!Rr%M|Z<(#%rlE&}HBms@1YxCsd%1%7_`OM*LZr^$C{m)( zB)T|ihoF;hiDD^SVzZO4sIain)c|ct8%)sNd|5@q3O?O0CMc{CdII7h5NW1 zFc`+eUiW~djSz}-V*k}>4R)$ajpZdjwIZ{4o8loh##3&-*t1IEi&6Q!;S`&zVAaRahoOA zKk;}kkB-$^naxzj0{{AK@xO|XeIO?(N7h2MBOg^tbnOG!{U+pI_4JZod@U_BfLBz$ z{{WX;Jy$3i{Kn6xqk;)YukM`7-@jVjyRl}oG{PRkec4q(X33*K^`Q!6l(wO?8z`a2 z%S-Af^~lm&aCkH6#sI$cTSCAvVw9_^YCiPtw`3P*i8|qJNy1}+j#2gloh>FA2{=b7 zO&1?Q7Za0A)m$9%-&Nhlup!5S2|#sbuAPT&R63mDzdYNS@mHW|&DZ>BPS36#pBjERALHD9|(Bn~L_*rdHYG^hGz`EZ$prZaK^SdeYbN&0% zz3w=Sw#R=xg~OL~{RNh4$gCL5-AH@hDuCS_A(5*f3hycwK=-1@>etdh;8&2C?zjNidZJC-85jPdxRkvq}4yw^QgH8#g1reHv9BzkwTW2 z5y{9dd0xH#sdT*U%u{EbM&qVTY21a4ZKE%zWIHVEbj@Ac)v5duN2lgss^k@qBuL}M zH$v5<*jQE);XU!y4s&z!4(JMX)}H6$X1=AX@yCX=mfekoPph&?qUH<&y$S!O&(CLr z*~G(BxN2`yQ@P40SfLk)aL1bt zgMd-z6;)*+hEaF}-+Ap{Yc%#*>oZPrI)*bc0i({R8iiS|THAhaQkvkSVa^e6%KZzX zmbLrWg)g4GSjbIgCKeh%P^osrJZnD02JgGoB23@G0VI}|@9b*F_zo`5UtV0GnQ3wwrLIhR?MM04-2$lOHn$=tvw2ndi|;UZGj1}Ta=47eTmh1Yai9rG{Zv0ZgT+0F_eSk7EP2Kv{cC|y}K}& zjTrld(|Z{cfG}BpWs^{xD}BvKGHwV}$mSn`bO!SwUWPQ5_Iyv#;Y@i%oRD!1Pl9|$H07!fz}@)V0VUhhK7D;NZud4)&Ju#_ z@e;8ca#t9d1oWfy{Dgm9D|ffa3>Q?uh_1H%c$`1#+NTaP!n$Fd+Uw`7(XWfn){a3j zCEjI=vLeYnzI%zysd-Qv)_Zf|$>;o9@m@XE z^{AX%O)*9=+}#-1J!PU${HEyIqF+yWX?(Bxh3JU7?Q_hl})Xh1HlR+ujF#&sAcnK=1*%oPUybh(JB%Ny*$ z1OQm>#m>fhh}wMy>;aY=ulrI=yd>@ETI9!{x7X7H?LRyD&NTXTC=Dv5J@pUEwi~Mx zMa{#?1Aj1kMpXL?d@kFbD|lHS!2z*tISkzn1j?7)Q;8YxW0SH9hjOuQ-OYZc&UcD= zN>oP*%Vpzf^f#BMS^JH>UC_%TH)y3j=d|gb|;4AgvLf(H(d#pG6YC3y%S&Vzo z1ZR7!Q}pc3&lMBK^OE~RXq2jrYMlf$O`rVxiOQ)kJ6$owyxrSO;vW>esn-v}xF1`- zi>IXZxJS(5`dH(}K^1t?uGhFuJm8yyPLPv;>5C_QKfh1&{VEH1sF}Y2m8nw|T&YY> zzDQ2|3S=jzi5=fWo`r{r*?Tl7c;8P|gK(u>3s-|$O@rujC`N-y9yhV) zm?4y51q?fXenuzwlLyT`>6FQcpz@7jSS4%TH7NZx+c`Y|BrX{wuw|0AZSRukAmgZhTZw_!N|7ee8{8w}=7) z{h+uPi#w}yKq8Wi>m8vDvJNr3D(7GKdZ)f_Hmi=jr+~isS$+BDC*Qtaw=t{8CR2&S zwM~4c8{=X36$)nX}6gk=pMMd^>-cj{oi)!w28*+L^ZCWNG?BqbU zUQ;CvojkaF`wJGgh!TXvtn)IeqZs=~d7fQ_g17If4`aVxWMBmv5?$PoMOC-ADN5wc z`M9jUYBY5l>p**E{*T=-s4?P$T#6Xl5rSL5=QG$0%l>wY%p z9{~zpIXG$oHO9Jf8TJSY?&up;2e3VBeD~%sb=>w{_k^yhKdV5t0uVmO8^kad{=VpZ zCMEhfc}P3tMV=rvu_Gw2ehG2i;=YcMQDW88u3oD=*WsC?gf?CZMBuQ0A1!-t;i6HeYdkjwKDt;T4|4IA;=sHnTz?6up`9GNvBNOmK{)K! zVkA6m|MBO>t6*vxRN|$j2hTAH0R{8?mEjCGw};!t2&nFXu9X?wzGPnSMspf|8bed> z)#05Gj}Gnk5&3z7NP)VUs{2Z=3)$9`#49}7isRoF<_+GjhU=7noFjk7cdxMGd%{yi zVqHs7R$d`;5#jiDryp&R=OZnU*U~<;XrCrGS0F!H?tH|0cOcD<)xF|y8ys3gSNlZ=W`B1O`Xnoov73IVRXKxI&+@uxQ-l zyZC)xAYfNFZV5nK*h68$=8YU;zw>CY7011oBi0{iZ&F3HG!j96WBi9ZBN1BfKGS>P za+Wx9-?Th$W?{Q^=#en(x_r)ehq;lRTgpv#yQ{q#)LFT?&AWED@mdaTz1ojKI_~@(k#%m`D5J2KfsqCO^49be}xW$h`=!T2-fZJ|%#)XXXbo^`l46j?M|mL#_(4tMSzX17jPSqa<;YL&wuSP=mulmGspEU!kBVy~NY> z!FqH7Fs|Wz8qmn@v<1FD*aiBt6aSUdRpK!Vo)N9}C(k;pSXFzs!(xg~G%6k>gzhx}6&SBNzJm=u@)V&+oWfz=+Hni}>D1)IEsQM~Vq-_HFVp>|14 z(U{v79>9vR-`b?M8bd#b2LxLD)D;u&!YZxG!$Y?QedW2;IN18wKvqoG$aKYPC0Io) zsX#th=-aezNgGVxG2HV*XZwjRTfNW{Hn#Lbr`@S`VR{8J(8REp^SHaW+3viPP74mj z#zr9Hafhmek>DUP@SH!3IqU)I0p3gHZj*+RC=h$RF0QZP{c3t7a3?!rnW`vjoGh`y zr^d*J3wn(_O6ei84u^f>2(W_{8*Q8y6C7C5f3Rb((nGC`6Qq0-fe<$2;r=Cn`opZW zaIo-|mDK{@SGEn<(}q?QTto0BYz6sjjb%U$fD$~WklR6fssR1`Ibn3bGwzrQ!tK%) zQXkp#wq|jU`yuj-jqoFC2u{Sqkoew4nBY@ONDMkUa5(V`XtssqD<+L#7V97%pfkt( zRxww5DX7X=_(NG1lG^vGwI(=S+(-+ln9r|HZj-|Vy{qDIKw>$V2GRgmN5cvN+Ptv-YI@$snR%BTiy{JC^fhwXaRlkP}F-AgKjnL2#Nf)wQV%eWd>hMfRsRR zXo9ivx7<<%;AP<3!AkdtEnk@aK6`bMg%`E<=RFI3phB$f_3M2c@&DBVfQVFJz3BG^ zN}HarfdCwz570d<*l0uk;*wEf0_|a1XEEs2$c!Mx_Uu3h+NhXkl8TfGBOv-Ix7(&Z z?w^R^j!E_x>F8JhuA3@uk$KKnL_+$hnuYp!bxB&u#p`1Rxb66bguot@3B@H%fsn{#ax&J#P&XBz%1v>qN}s};Jc0U9T$07ofq~&U_#Y-z=KIBdcO$Oi zQ3rFSI=6jWoYqxiH;+^a09BlpZ+pc}-v%0oH zy6bM7v3&hAif5Nr)V8=o5~8G821r#AV%Nm3kQ~YJFwfDB(?z1Bw+a_Cr{>%C4JTD4 z44`jvAw(NenM_kyD9GG$F9<$^f`D+Y@HN2mg?#KxXsNor2d~BbdH(@d`rO>J?HMf< znk>uXb{*UI=XbXVpT4yU|GVCxaoco?)!`{s$d*_f$Py>;pA7wB8k3Y7-fc5W%Slt2 zrn%N9KD0MGhk!+d2lKr>V0Lr&>?t)-&F{;o;xr#1Xf@_?4i)niA$=i zr|HeY#_-Xr{#y1Y=NU&xFRXj?5c&pt@?T}OrT-r$;)6GOKb}@Q|9n3>xUw_LU>p+? zVmE%|>T7(j@X^$OYzR?U+WZx#%{)^@%1l5DrgfM2H3s3=*JO>UbU(}P`E_1wNrc${8jRi#3;(G@L!KO}XqaCk{4 zcUX0pjy-1Em<9Z)GZA_<_rX<5`Ce(To$(&w#LqeogpJLg z+D`?bv?$2HT{(?1#V2HPv#EBw#}LI>hV9N%&UrM6e`F=G_Zp^`j(~NKjVO%Npsf06 zH?XWn<`~}QNJiHiSBv|2>=L|OBPGqVn5}mEHvd>OT|G9icQeIyqaTp3@Ylf}RObT? z%61}3O50-x)`OxdG>}TjYg|eKIHAUtr+FSL@oUd`6i0?4r0xzFXaz3t zz90W~U3e9WnB8XAVPIr;W9VcyDbn3?yQ(unT%Q#Ivx@Qu8CiqHcS}r<^TdGw*p9qp z!YQ(eyL-2ghngrUq%v2PF5i$fg|TwrW9ES8CaF8p;HpX4%%|mfW{Z;%tML{=1Wbbr z`b%xhbKiH}GT&v(Pr8TbFONfDrAQCkqW+H_*8!6P&!JR+3WD$q1`{?OFBMJyqgoAB zh$0M8R^_|9{V=M#y&OTo%pZBd*Tp_LzJ)i2@>mBG?sAZtcx&e$LrwqaqX1X=+! zbE!D+PdPXUl9O75v}wLsru&3762n(?c{eoY0~3BA#4AxQh}{G^SG z&TysOm*y^VQ2y?eV&fd4%0@fvpOS5>@7Y~`PYVI|;Oc*ai(BuCI7Lwm)EyeoK~IXt z9zSjUJ3kL$<6x&sOA5v2;YcsY%^UOSOnbJMh=4G#c{{jki)plxwBN=I`S;p@S(h@h zBCQMcvZ2bk`_13#KjX9zm5z*3&~Ey@U@w1l>M^7tpfMPE3X_s)rEq&p+Dr^uqhwm# zu{ks>+9&d6_L=uK(evvy?{KYC0UJf_@H4i(fKGX%(K4Qrb@q$R8%EZ+$pB_oU-Qv2 zz|XzH0D)v9^m}NSz2J*3$gmkZY;RCZff`b&lcqLC8J{i~@p(iI#TwQLNp2G*(oOeQNOD%22yB%1a!rM8;(^oEWzKCAWb5S zu*rCD!mnY=0=|Z?3hB7aILlLy9n>iHDBQm7150;j!j;~X#v#FuDO&snZx{VLGVtjq zjbs3S5Lko-^ybr}1BLffo3i`()2_;_a!TR7T75yeZzv z1QL$AUc)Lh>lZ8YH#g{8@~7}|8U7oW;U18zBV*de&2O}+xCQm$H^_A_J>4wdrI2Wx z5{n04%>PlnNj*XGQ<5r_wXMfjvy*9_6+VBAy1%rLnY>`NznJx<*-b&JwB(R^j7~v( z0E^Bj5)96Wyra#;#;(VCIEuSk}g2UP7RS9G9P(K&6klD(Z1Y8S49o6?B-2E z60ewMy0F{W5>fK!&%RU~AtJ$<(enm!|EPKYXBNw?|L2X_W6~t+5un4McmLA;0ovs@CSs zbH@)$y35z7$kiiX!jTx1ehO)wCfmMyi9?ZWvn(;r)Zoq@R7DI~3B!D|RtQ2F3D~Ue zk9!ncd#U-s$OyvOgA0+Hk+ZrL67j?wB)(xeh(+TR(+)BmI1s@hom5aTkBqigl0}HF ze|I}#e$hHt=X{qxfV5#fvvDQ2&!fk$uA}3Z5oxapxcy)amq!o>^1~AC(UOZb(w(^$ zN_!R-vtCszm;LxZK2u|47BT*w$Cv9di7|dR?V>X?Ekad$dzB14Z%Pz`U211OXESKL zTX|(U4tWn!-4fW>Iffq}7tJ&psDD&;H{SQ&E|xmq?K6aCQI#<&b{l;?-gkZS#Jjf_Ri?_l*>;Z8DKYWs#xcx-RYc3YQU0~TwK*T5E5&YE zi7=}_UDKo;$mmPaV~_AmjlX@bn}9eDTMT|zqy*>sRiUfKn#F5?3d7u5*!osPT7w{z z?N_ZFE>C6T?o{$qbjlSRF>QpG#jl}Fg>w+J0}NrdvX5|E+_amYeYQCVzM~&y8`wtK zD-!HU&odHz#x?Az5S-HK;H09Z6Os{mr1l1zLjZ2?QVCO2{xBxJfLaz%`YMEJlnhiF zNm47W=6--;?8;3NQ-@hl&}E`bW`DYP_x$!qPj2GpvupxyzeI1GvKt5=J~39v``sXv z%cYL6_XS*1W>9EfINZopoMlXu{#GMHodndF@uwT`(VyOZl3giqkaY}XM?W zF+4sFv3yGuUEy4m1Cr-#hreo1t9yA0=+^N{F*OC$8&A4bbPHOjz`g#Tp}y6eeKYBb zJV1FKKM({XOVh{~Yka?iS*PnOyizyvv|86&xUYCnwYrBN^cl+s!(RcX0NtYg>=Vo; zVCu?$Dg1fr-+%CDZQ`dQwbKineL{PlybRqTQ3<#Ve(!d@DB27r9T^#a*n z4i;O?NFB8dJfn@Uk}jF(bT#hLi1RjW6?GP=vdv3Lb?XH2wbjMIg^z9E07qY;D%_Z| zXhePuQa3VPx^P7^;fnpB9c4byVE(3ZtHC1uJ6#rihX{~}Xesf@lY%__en1KCnvLW;%8RMnuA?G)i23%S&QvWX0u~+-%$?Hv33Hl)1H<^>X3cbTrAM{6hSSoMKG04qZdIJ40p(gtsCSI1 z@~q`Tc=8l!$w24^O@B_m(o!p8>Ko_lw=%%a^Wyt`g?nPuMosnZyeNPA>rB+e+uYa61GwO8Et1c^LEvI`f;s@VRHm7-r+OQ9jiju(qq>!tmYIFZx> z*)H)oMKl{va+gxTN45*}GdUZI#fRYH1u& zt&FM2sf{&$eNC!THQJzb}7&@ypZf3Vr8{5`1RYlVV9GdufiO6 z3;iiLGn;?lW2%9S0vvjxxYKXhiIi0IKE%{F*3*bmQWs)t`8O&Ae;iD_mz(7m7`WeU z$v%Z1de$*~1)IV@BfwaRc1rkvsCw(LD!1+n6hS~br9(DINK3cCrlb`lBovVD?vU=1 zkPf9oO1cHMG|~-%ba!*t=6t{V-1|Py`R5$Ye%HHV&N0UvbFLq&2l#LwNBRKk5PZ}0 zz5On6JWf*r0}3)cK@5&(Wz#}RrI%=fVFR}WZdGtgP;=CKl+|Xh_$JvW*|r{Epk*+6 z9%(=38G^6A8h?=IjUXiWi1?xv?Fg%5Ln$}V^K#^0K~ZVka7aknBRJN5i~yUdk%#?t z4JERESP&3zXv@V-0w=^}{wG^P`F2IuGZ zgYp~mrHl*sKK#ampVRG!bDG`$;niNdzr-N0g+{DshTTu5FLx{nzkQPh$WcT76jHI) zB-^iLHpl(MlmOL~yRvxdJ5dWOhoHEydLzUQ`Gp1Zb8ZOO6!1g8d~$coF+Ta;XaR6n z@<2BL#;psMUfX%&8GOGREe?K3$E)0Ul~M3~(#hq#F?)22Rq^?r>{YKmnt9e1NUK{ob#sq`!(w^FbHe4EfOm=a$ zML!2MgP#Z27bk|4VZqMNs%Q-@a z#1&@cAV^n@KrEJL|3h0Nrv>Fj(#!0@AIfZEbi2&B&&{F^b-OVxdkvXY)!#pd+xr)A z*P)|xmB=bF0XnzVS=8Xq;Kmlqk%(y5eaou^*PWd2#)G*a>kR)zp+8YsqvKkS^LuWXfjuVIsIT(?F(KIyeN6dLtl&57S>=tjQvID6ujN+W5h9~7b!Cn`)a zGT(^dhR*(!wo=I4U%O1imV$w}^w%9|7gez~>GFn0ZE|U-;wSjjvhJ^n zLLaPgeU_hcnKNFs+);3Q6065!fYS}brVaGmT>f5QRgeHqH^oDM&nC;68r$Ml72`s} zDFMPvNE-tOuHEew+GT}$G2A7K^)e0My(|;?;X^auh(H!8HDTUwd*XJgWje=?{Ufol zaS^LD^&dMmvpMsR7lHRmv^jk%F2;b_;jO43X1mo~-wZt%R?%|O(cbez0#2?5HYXxH z46d*OC&MyVcY8vDh5BeVKcK%BCE`sh8QuhUZ!5^2(AcdG$x(n+<`y7+tKpHh4$RgOjob zH5uUhAN>KE=P4xH4vuyrA!Lo1avc7gU0T^?txvEJZ29OP({;9sk)D|eP{L0Fa>~dB zzIn&P6)g1>1koNf zTQ-_*aTJb1dtJ>jhxZDiqq`|ta5eAC?>&Tdjap@ijEyQmHH8)cD?ntjnve2v@pvLH z-j!~>^E}3K-d~KaOD3saV^d=?FD{C?Jsy%6l>Y@&Z#wP3kOh?p-XCeK_ZF$O_tvYJ z(|aP?qm)@~7?c;ANN=e;aj9_ZatlQK+^fmdra9GkTxIe_!P_B>`RB4meD8Bk4%m#!X$7g^#I ze_nry{X#dFvqw%6vavT0242}5Wi@N?@ZV;#&pLxUJNx<|#e5_&EuX@{j?~CEY;?{O z??e^&7ejdwH;B5VOfGSF3nu?ec`#(zY5jN(w| z_R&dVE9(|5Sahi#E7EOV8n<}{}q%iS}cJ8 zuRi8@dYV7!E7K~abVLk5q_XKV-woE4(6)>FBaxCU^>aWt~sRn?qOm^Ujjd zu~?MHGeEQ($-oOYX=e{0ojAuig}2(vW4zBJB*X|UDq6<$`V1^XSIDYLvuuO~PDt5n zg%6l0-GIlT#<_bEKMjwRr@0<1Y{0aUg!`Mgm}7iSNTrNFr~E z>7uM$O)9Jj!uy==8d6~ry@3=hqr4!LZ>9QxDT>|xD`jxD-j;^*(Gi?H23mF;4$F`?Jsu$itMr>P}?kAZN9)IKx1fP&3E@I}ECkhl3+i-FseW z$gBb0zS@BP0waJ$*tZzRl9k|gd9?*&%5p#9J7s6V(J z9=7MZb=S*Y!5?1=OtS8M=Yd<(8knOt<%9W!Ii+6=;lKK~`l*AKYxwF+6`yRR#+-VF zYCg9SzLyu|;^}Jbz1LrI+1-sX z(d=^=1c0q`bbGs7Te0m^{OU6z66bzy^wRfHfYT#GcSljZ#l;KQnCg?6!ak4HSd6T< zTm!g#HP7s@4lz5own2D;iT+FGSoHH?pFm4_fi4|U9u9%^4~FfRlg0 zbAykRLXc-K-vG~1Uukd8hHdsJ>svKps`J5a(3!%@T96XxpzAhsb;@!3@jFH=X8q-* z>`e3Z-=6Q!zsqVT(N(C9zG;+Ynai}mu^iCNm)x0JCerG+l8oiw=9G)vaCbuA4k(M& z4ZI2V5^F0tNEd~cMS-(X`8AEpq>25uuCjs5&BRTpl;>-Nv=LpX7|Rhpr@cCZgM9!C zkEC!m^z8defvWAOb_?E)gchmN?#%N-ypjEv>MiAI{LUR`QYFw`2XE0w>-qrLDH{`8c&S)}km2N=vF? z)q<3NQE>p8lzmzAKEZ9Ya{d?#6ROkdE#YlB{sLfso$#%!cSGW(n1an>Gm5Q9NQ*vb zrP7Otk>tVEQAP4lVmj&2q)&shd^q$~4!iaAITmoB97`ZnX%re$18jbVs3>?no~~Gp zeAghAP|%;zd_cb0Qv%DX)g#ipk5~D*7txZxMI6o)6T{hcfx??rBM_ex9tGH~Je+UN zSlz}PH?=P@$7-yxPK8+Jv?qA@DYybj*sMZX8Vc+*nLZyxBk1=&FxadA_Y0jb^35b? zYh^ThqTp|z=Ld}RW-4E9`v-86V3+FS@3I@pW+0;tW_WWfaPzcoZAK|}t^yhmAvfN#EwyDSmU${l!8-zQK zU~CZVm5Dv8MBmGQUfJT|zKDdy!X6}~??6=?T?z;LMi2P%_t-nNrC)0U5(}rGy|3L$ ztrx`JGF=^T%V~mv9o5`m!O~K7lLG6{1-O z9Fn54lgjpxxYgUDLH4$_>x@?;3z7w}{`TL2V#})?36&PHkpnwCZ#9|TSTpu0_jtp9 z!3e(XV4=xmWVVjqFOsg6YUfduZES#%4oT5lw_6Edap=JYUj+h|)h5@RiG~OslpRmN zz7`i$!;Wk3Sqkxf<4+6Ycg|oD9CZZ~b*c7-a)^+m@jkSpFGZ3T7GJ=FDC7hlXGYE*uFVRN+heOPSKXS^+ zO`_tQ-4E!#y6o>h@^X=BeHF^BU4W_%oMVSzR;<_nylRATjMG_4`OD`_9-CeKKThVq z*C;_8)|I~eTSLE8beW?8s3F$LiAsoQSaTsx7qCqr9R_##I_NLZ16b;`I6#&J%_=Rt zzC9!&@b8{#X*nN?_W`=2J2;UrDv)`@(st>)j~*YJ{O~*WIXogLeZVP?38mUwsJ(HI z4gwwr6zDWaLRUAyEp?{j#Yr*LTHUoF_&k9(VV*SM!!oj_e?5EuRDCF^m=h|p8_inW z9+F%XlzzipiQ4YnPBL#+9mB4y7X=!0C1~KrRz0jOS{jIXzUOyQnuz2n#sfbE<83Hx zGR%7R9Jz~>6r4T*;u?aQIptYyM4_Kj$Ihj!m1RC&xXVZ$;N#AWUY|1!n>i7E&0g>Y zC?@-}2=0*m$U-vB^({7#(t)h%%x%tN7Cp&B>CO{qs`EJDVIb=Aa6zPn$K0bXr* zAmcFXy`~HHZ2FTEtY1@uhJiQvxngJ@xuLMj2fBpI?dHNk`&{4V<+v$#o!1!Sg{z0W z^}makad9e%Nerva;7UMXv#yDVDI@oE8+2~?S?C^jqF}~Mg;=ZAh7trm?CdXJz|JmX zd?8HXg-^ilO+iQ&&`ygYZ#l+2_KrFLFkiE3C#8nNyNAIX8=V?>6Rr6>f<`8LN=@Vo zREc5?AdlMCE;_Y5jpqi-MCH6aa3Ee1;$g6;}%iPV}L)H-b|x@Lm9HUv9m7 zVV2x~H+mB`JnnDtQ)8Lkyv{5sgk#NNMJz{D!Ui1GR8jX{QckCSV&HiE1X$G2GwJZ+ za4DMTa_h@@9oM1K<4Y7db7|;AH{9pGz4P`a5wRo*u2zJfQMI(Eiz%?tYG|axsOd1c zUoC=)H$qaIVQFvKFM!i90L~4PY;S>KYkV64f8g-hY-;1966ZH74xS~QQ-N>=B&?2; zk8m88Ar~6Iw7qE&JD$2#vDAhLpBbWuP*%k_7w`K1TAwERwF1;t;&TA7uJ(@LIPFnU z5ouYeKUK1vV+5Gfkhz2LA};*(98{(5er1QBR(8EXgDl z?sNu-hv9?;&7!LT?aQH0uL10x=!UygB*97}G>}^iOlt&eje1Q;q!d7v-~|6v_}>p` z28c>X5cH^dJ|ivE@0VO2Ld{MdsN9b6PaXb*c(ULguAoZj*>L<8K4&QpbH*|L{CQEn znc?-Blo<{0YV6w0*)js(YDe9ocVcFaK}BCd*T;_x@Hs z-q>35ldv)<_F!Kph6}?2>nfAU+aI(J!C>Z%6d(L>@;s>73Yr1Q-GO;Zul-hiV4)$` z!$lhG+|dz)M@E2IYoIq_7AR<@((0RBzNi(`He%AZO=6s{L(^q=26-hse)lhfUG+aF zS`r8kE>Pn#(<0($YcbrC4p$xs2GCrOa>P_kc|4EddAo`pTQ+X&3MaEMp?Vl!uY-z* zTNi5d-z9QtG`A{}!z3B#IcF4|jayrZ062#ZLQngmEb62ZMa1qg#WqLfAP>@(K} zMl9A8;BIP1`5KRQX}td^$nvcuQdFrn0AjI*Ohqp~k62d2z_ebtUdfsp-4`7X8k~=o zw^?$d4;l1C#SDj}sb)@z%0}S3RNT8B$b6aj^01g~aO6p~YxVQJmc`$WA)2Co;Q_#d5Yz%?2|0z-s zd=qJCF7EtsvW`!_%!Q@ANwQn9bv-mlR?G|kn%zV}DkryL%N$HUcCv7>L2EhHGEa^m z0oaB;7l-WQKflYIbU`h7AP7gv@{<)91(R7;+e6bwRxVq?cuKUqly~;*2`O^y%Zy*@&G-u zaes$1Y)V%Hu@}}75Fie&Mo`hziBJ@BHLR>U#6p+2$r6|F4oC`%eVQiVK}60rO|7q^ z8Bx0Z^R=a;A>bQy=!n|HtqcjP5cijeh`!oDdh`_XwLhaX2V#`uC0{jga?FT>!nJq}8c-9Wb+zcE`gxfFA11|MUbQ zOG-ir^Kuny1Mws-HMZ&qbR~??;|dV2dG>Tx$-2r5WJTV96Bj#BU}fpB!)iTY!b?`M zn;Y#bwlgwzd6~WMbuMQvwc0l3{KsCo^ewvCB=*O2K{Ns7aeUp3tu3UCg65JeHcC$G zU15c}2Etm;b9V0VH6{JZGQ0F1nj*I^F+Mfx%wwe6z((ifb-?WrhD=3r&yQ8c=n zojPYCg&qX35Ij%e$8SeX-24SLt2W&#aOA^oJDTw0MQ^JZ!PA6#(4lV_r!d=jzAw2Fbl3YAj~1r}(G5N_crC9zPDc$;r|39F_d zE)Fg|!>zYiWPIM&K+1y;$j%0Z;>MjU3i1Abb-xw=@xEq>6>wV8FVGN7Ui zyU=1n)*N6IwFiv_r19yvFgnN_6CAN_zK_xS)%PZp0v^vpo#{P5Ur0cQtg;F@zKSk< z3r`T(>wvIngTww?47F}OzHzQ9Fj3dvontivGKdZFrmIMsjh}4G5i(vluIH<`I&sBz zK0vW30ET0^UCLrS;2c=hko~_$cq2|5f*%pN$@(z7&0oKcsW%M%2EKFCGO+H-7aXab z)XYH~E{`F?qm{s(3Le8^Fbj^f7X)Hpfv1laJU}kt$z_`ib)aaP7!ZN$;@tn>_;t3; zuqu`qQSCth*7Bv+y0HGO;{fH&}KYP;((@5qL-CXP(b>aAl!q zk#PGh6x-m;YJax(^k}NZYXqDR_$sH#)C%q4R)WOrf5{Z1Of}S+yE35yR>i)yJdeAu zzqh|ZYLFef{}N&SZQXhNPIPKFRy3BU0!$ugoHm9b;8!->;ds{w6@B#2kCk}kfH^?I zV|pFQJ-k5PRRU3BrW(&4_?#QlG<#T=7-I{k!I zpKq)hpZ|oI>lmm(u6~{2JqSO(H9)~SG*H1^o7_8fwE!OW=ErX({SPcR;*~qzmnRK} zg}^Qy)A?0lO1le@05|~uxzjgopOM!rt9sBEZj-%c8GFE3=Ru4X7*aL}e30LSB7ica z+Xv$r<#2Oi(FH$R&{GtoclRsQ9AA{1S+tX>6N zcy`@>Tiq&uYc4vtH^Z3-XUe;}%x{5q`?(R0)yE z*ZSLmdtcU)ZFD9%*vQ4#23#{cUnx;K=tOa}vOSS~*MSTmem!=LxK*;Kf&@W5NHxmd zy0^q^pWJQSpM~3wnyc$c8HwecrlB;~{u4SPWOQZU6wzamUPXW(yf~xD^~{wM(ZlK> zeHC2k)|;2rg6RS8KxoDK@x*lb+M>DLxB7G&&dTDL`_DY_3J9RLPoT@!+5z2^BU4el z^nI{JStWrTZ*XtPXsH9OyCgF7GotTzuA|{wlokGx)KHu8Ufa!{3@WN87$8WHDf+@= z5mRFC7a5#H+}(8b!V^jN_%AlZ!eX|eCZ;AVEN(c!=n!-=Z~WL9aI`&EfO|T z6B-#Pom&CR(H2-ZPk^UUSU|N?F%IHPw|M?}Elf~^o09d~IcY3pVu zaeP3W*KL?O?jK!CanOj1*L_-g6&fiUWKF<;=tEMXPV{tn%z9x>X_D>#4H|$16wuOg zmX2OP+@Lx@X|v>+>-fn1#C?+~;A8_mbNHE~&)JZyt82rKHgpn-gmuSzVl!;Ux+re& zuHV(B&x0c+GA;!a$=SAJgUD*Ou5RX&zE5Pj$9ayE0_IFlPhXjl1IPqvo^KU%q$+e& zwcrT*PI=$bRYUAq|9^1=*5}~h;GNe-h6A~|c=Nfa_UHSxuxOU|K@${nqfV~>->8Ax z6IWQ6a{KzVoD&)*7LkqT=l@bYrmIf}Sq*IUp_qHjjVL2nXp_N)#`|9{?9Y>k+y*Zx zSonV+aJdh_8GW{3xYN(OxxL|VpW;ZO_>(d8yS*|$Q(D)l=Afjrv=(CbhYqbYL?=2` z+TS`|@Z-skj}(6zet@Y22^sSjVWXFcDhL2q%{?5HX0t!bG}IO42mSZhpA_t8&EQ`d2mLR z;7W`V*EQ0yckj@LKnPy9n|U9QnwV-D5@M+ca<_0+j*ST@Ze^(e4`i=b!D_%+e{DoF z#5mXnwj1yaIeia?J`DZOpC5mxP1GkTFMyWF>~Zpp;R^%AE|*x()^m^5q;)u_9{KQtuaBasx_Z|$;5xF7D&LSN z3ZDBS+~7BefC8zkdfs~L0FcwWq-Zk&@T6`@wwI~L+`_3djWV-rXOq`403Q$@xb@|!=%*C=aYcRaTT!Ou zIsxG&B3SeyZt?TOPr<_*p?2Fo-QYL>0-R@s#A%WhbWnPi+p12KV$XUHe@0V(?GMj| z*{Ygl+99z~`5a>-cC6z3Ibv|itspcxQhMj+ytU%4X7WQ( zIdKMRPtVGk%YvWFE8hTTP!^G#y4Uo!ZmgoLfsCnG18^EfLME@f!hhu;r9143Up0N_ z=UA5{36obzNr^(T@eC-^*;}WZ1i5^U?L;lxQO&5&{b8gw)}938PJf%pphoW>KG1I~ z4xh#2%8U+Na(*TJO;47HLHFf2f(M#o)I_NN+? z&gY~?%DAXl^78lFI5NE7vA7w&Od@$t;DAeX?kXE@KjB(cSRNS}*^iYZeU&%bFB)IR z{}yq@6$1q@z<;V`20QJ9pxHDdgWO_dDxRugl!-A!fE6n&2UaHG46!>hT8>Ld`SXTi z5mqZbI{q8hZ!a$rhu*cLxu~JD45O>wETt%aH_N*+S%6-y+G)C{@ixd^q}nvQ!=kz&W`q^qAKA|CLhlA`hbPw z6oHuSv!H<#WE^&O@><%QZg>tN&UqgNAs{edsS!uicX0HrJ#$Yq`hADY9y21U*o8Q&NVWOu6_K;A66 zjl2bQ-yS1;ct^;h4ermxzDj%}DTVw_^qk`C_P~EDW2Qt#_cA z{~NOJ=f4J)(>3x2w3Q4Zc_B8)r7VnXzpZA0EP+B=PyEq@&Aj{JK2jHQ*HnvL1NU zsT6(iQcCHfyIz|;Pu10711?P%6aa~JMT7RET*26dqURXs9lu-_r8H1QSrt}GUMht) zK8dCh1G(Yp+@%Xw==lcCDk?GC<2rhOvg|Xj=clqo0rZn>t-|^jfetbHxJ@EWV)^Ig9Vv+XhAf)e16&-v)3aHaH|;Twz=Xx9p2>+w)(B z2K$H8aQ2)v9|r$>#uuD;ErI&hds2* zi2kS6vc2hguM_OY4=;=M&5n-Fd!_^&hCS94mb+~MxD%dNcOEAKjNSc)D*T-?yHG2& ztmB>hTg6{7-Xs_8%p{!qh5@JBOu%Bzo00z^IzO#fhA~?(9}e-{L#-rB0ls>89pouU z#8sTw_;lvgE=VYvndN2@k5)0z`<#8X2_niR z9pG>`w&uRNGT`Up2i}ZbzI8R}6H=itH=~SQf!o6ke7E1f;W$C_WL+%x8Efgt5GWNf zJYg>Xu#+YLSEPIdFQ^QpdI4_q!l7uZ?^gidl9G|b2xIOeus0C~<%Ak}ekfHk{nD~k zxXaUn{pp%NGMRltEd9Vl*}DPg3pkd*OB|7ar&Y!@7VApOD65OEu5Q`;x95giN&1kV z%GBh1K+yO;l$!g=mF;0=w*Q>EyiyKmT?A?_!uyrigI{I70((IAC&E5LlknB2jcUQ= z>2r_Po}po z1m&JUSBnB7MSKQD{G$@xMwLW{3<)}WZgUHqoRnj3*4SbT$}uwK#iQyNW|f1rJU(@o z_XMh=aIdNpX245pG06sZ`8Vh7sCJhBUXPJuCV44>x{>6IyK9oeIfnx*^h(vY5J+6< zZ3rGHCh35@B}5J;VuP3VY_292>CnO1zegk#VkB=p6rS2kHb1)8yv0$QREK{FS#>1_ zE}qtVMKh)&Y)T2BO@jd=9&~h6?5uZ4I=fWMq-uz2HM&3qU|sw>c(z;qmam zO~@voYHfL+Bb(guC5vRHdnuMjH*L~8l8RAPcwN0(>23wrK-E`*^yy05&1FGpQA6_A zNNI{>F=|OF9~}d(9P-?sS8X8nb$jmuR}w5eouaTKkcK$VJ(*4jN&AdK)=wo$?xTC8 zk;Yb|gXi+>9HC|5n1^cXKIWSeY(8-FOZFZ{=*`UnC7Y(xZvt~^|vVz*^5Hbkw{e`)os&a z9D6>io(z+91cV~{V>?k+O270Oagg}1=Yp&+?QTXU$+SkavfN=5nHRHUu!=U3u-<5Y zFqjODn*sF~@HwFwQ`g897Nh=md0B9==;M7|S9W#fLWc~M&N;5L+u$&GEbQf(@`DqE zS>b?SjiQxL0(bQVhJT$l{pE*@VS7xVENdz4=+%q$uVYwhyG#lMU2k$NG~)F`Zp*zt z#D3i-x4oOP1FuR^0YHX*?g}wO|5CU=Z2Ir?x{z3Kc|N=Ku5B@Vf_j)xYP3t=-5>R_ zFETfx;_(!rx1r$VsuJjfit3y;tkwl%SA#=?w9`Ys!tD0HVt_UWf{d}rv8SjyEoU`GyNzA|vWjKv-qeT{w~5DXTjn(A30Wvqi; zn0RS^zKJ}-V(*?fH1c?`*L)-K>t?npVYsl#D1EBuEs>pF$mRm&h=}_$7Me$Z%jdlW z{d-p_8s_a#0d#!jHL&lWz34oSeLK4d6a)><)9YQgx21^hr(0l7g2Lw*U(#WOM;mzl zBP{f9fFoLW@tB+irF`5Id1W=QfiTi?8&AseNH?rd)raN82__#i2axtCSR4m$TCBW; z<)f^H;)1(KPJ@fg$^G350f}i;l)qUX#`Yln$AUh%(YZt?qs+! z;6GOdsi=?m&9}^dh&VJ#S#y&f{+UTSSi9gGsrV!#w;Ph?Z`f4(55J|QMF%3c`hi=T zmKzJ}-SzRjhzKCU{XxVwe0~Rd3g>73^^*$cFgUZUrWQ?;)P^(^I2FH>-I=@m+avDh zM ziM6$Csr%YH?9%dDxHFXmRU*@PLpdgn%F9|-lB{J zJClq>0h2Y~So;ltPM#*NmrD*hH{qw_f3_TlgM%fhsiMF&Vfyzj(8q`!oy)|8zeK7) z>0W=>=gyuen)4m4IbKIbkOQn_jzyXl^ASDsK2MigTic5QCK~CUqo+P3G$m0>^@rGL z!LpH--}&%tP{&)7)Ys}cg^YI}a*3ku5S?#>s%R&8C+$N)qMus_IP zLoAb%z&^5onrsaljwP|t{M?^Ky%3<3c^o8iTm9{w5l)1*S`up89z!}qn9@>t_`P{- z;AQag_18stbI095%Nf6jK(Tfewa-d8$}&K1Qpt>0pO$h()~_v!@pb)|*GTq&1m}5F z{gWe%xZw!c#{z6~N^6Lw3Y|1iXA zWSAV56cjxfx;epIpt|P`u7OU%mYqR-2Antt71?hoPVBi_yIXF0z2KNS|!3V}TGz4@M z8(Tp9F&k8!Fubaf|59mrasQ5dNcaXbZ(b1VJgS+ve9--Ac&mPdwq$9w9@NnZ{QTiW zKrQYPI+zk!w1oT(K)3&H>pu?^|5;m=l^ z6U3b}=pPir+qiwX7^e$;A`$ zzC;F96qUueu^940y%P4?WInI5$k+BZLjj({xCBv^N@6N=Hs0^JSf^grQ`r}URbJlu z6-nX0D!D4lL9crMN9*WAJa#~fL}a6E(@!dd>i-wDP*gv@5@o2|oeHcgueG$%A{=(b zZp#X}{ka~mcL0Sjn0%2aI0|1sn-MVdmPtX`)&`=*x`TFSvl-DlBqR>zQK?_8Z!$F! z3H*Qqqoo|F0;d1OZ(d?IzVG#2#SOSB6V65NDT?`Q&HzJ8^N}X`dRJ$o$q|&Fs5Hu! z0C@@bvAOXm6|x)h#wRJ+S2rM+FI=<3R3f|f+z^TY_LPxizP&wM4wqAe z4it8kTVUZ{i#ff9MLk5F0;y5C((Ua0Q!mvBcP_V>1p;Blf2t}2SRMk;Bomx#pxSe~ zo|rrp)ECo#9EkF;atRr4wmmDU{U`&?_-YzBS`$(l1XS5kH%L#oZx}INx`rCTLJn@zMkJ&9kWs-NCb>Q2$Vulo$(mtOA~b&|hbp{yzd9 z(=1-kyKo#LD#rZ+_M&lYlwBn9YxF?CyJD*ohm^~EQQ5NM(oXWB=Jt=QvGJgq?J(cZ zrz4zJKOj0kdTYHzPRn1$+w<}7%ip}#GITiqGaZTr_x?EOTAx($qDpm`vaw=sb?BU# z0M);;M-_bD;)X+zr|@brLpR%|Oa|)s7zLBu_xj#mtVQz*nO-T?^^k{R0$(80?-1$m z_cvwSNr=*^o1PYxF>7X^3va7`*bF*ggesf6eZ1uUvwtN(Y5`Ps2$jyel`x$C%;+dk zgD^U&@rKJ@{mec#Ipv_hR>8-}J$EQyl=~^dkDb;&gd5Qkue*Wuk;3;3sg87z?XV{y zZNV;HAS>s1gSqjX*Si?Ij*!L#Ure;T?q+@3|wdme-ulGZqGXh=I0^B zr9~Zn)IK1J(@{4h4(&qikvDbl$`j>Q=hwQv+L?@Xr&BjzhM2z=EX7vF@<50GBUq|* z>Hwr@hkAcJT+OH7^6S3v)(4%);e;S~cq-OnV>kF8l7bb8K)HcW7F!J0paKgEc3~Wu zE;Y7As1A^tS$CPli%Cv~ltp?g^-dECpRE{PoF&-+8Aa_!A;>nofDt?}boasJM}SHT z@7L4_ZeH{OsnmK1yjCA&wr9rQq6@69MF?idFQM92f34d7&kOS9R|3*ictDWd)_F|b z?Kg+SYb|Re^1tzE`PqDU28Y293Dr%rmQV4PlAt6Cx1Kn3KBN#wvFq0+Si}0y%e%oV z+nf4^&|ths??F(1;OhPF7~b(rmT5`KfAs=>LcvoRm%7(NzTnAh(*5k$CdH1E|y^)Re=j#kBU!M*wg??B5aKCxWYbiU%{eMHx0kWng zY9jhokCq!CF}8m$a~v(v>Lz5wMNbS za2~93;4gV@ZxH(^*43m#C|Sgl7lH_3yRIkgB6T$zkax9gH=K&CM+)okD`j>&nq}ZT z8jrp3Tv=x7q9-czW$15SRFNAfkhAfz4qJt z%;xK;=$!t0guAb-n*nrmK>-2W?ikIF+rP^Z@Q3-@zeszMR={v2^ti9k<%Q+}%abrj z+8Z?$T-+Grjdm4$cL@o1SqU;4kw&3bG5kc9)Yr}~&Q+P`+g;nnEnLU_CWjHz`=ZzV zb3L4=p@j`zycVIBod;2!70wgdEMBfmh{BjYL>ew4>w*i@*Vq|1cXB0SHB^O{g~qSt z-h?&dg>xO699-p1j(DlM^k0GOH+1h+eS^-HX$`!8Prq^ZuTZ{Em5pM~-B{GvCJYQ{ z;4-{YGh&jEVK$f4$9vY+B%DSe0U&qy1hHdnkcV+S|NAoN`hx!$Vu*HdEZI8TSf%#9 zq4pODm0l+oyb(8hgztcVYL580iraiNIzytrTOV_a?XKm3a&D3C+;eBMXR73RrT9zn zppTS69|u+}4$3R;C4|bIoiZ}DVnP*TQx9sfd^$1S>$MGHfcjnS4|&yju_^IkV-&m@ z7Ds0hwKPu<5nh$58tRXU3g32(Zrpg9G&S9Qf_7^y2zGz2QM`Uvs3!6#tudd1fQo8C zE=(9Y_GEfm75MJ#VC4*`?1yLWj(aRl;Pi@vw42Fbri`{UN;ft&HmZk5A#E(y^x;;q ztf-fE`P$M}SZmpP)L}ZPUTAq#CT}&cGO^&P*^Qt4Y6sr7fS(M`%S+b@3| z!|KzK5DrY2{f2#2PSE!PFLT#Aa|45a$+33IZ|ilO2e+1=y2iNR<^?PEoc!1{-5#Ha z_-Uz=Do}Wv^!@F+L^r*Y@l1 zjlQ9OXtX2^j&|gxZbh8Irh1SPL!4i4p{(|x2Wq8lEMz( zh{A6c4QFv)34QcZmwF^|T#l?;gnmz3dit}}5TYphdxYk0>C&6h+J%>=k&))NSEr?q z=B_2|jfXOt-#jdDR~H6Cb)p~&khrL&&CS~haS==(9=e~V+Sxrp!|;!9Y=r(UD6D?@ zB}uSNX^XaD+x(ewx8yd=$fDK>wu-%U2V zxR`R2=?M6M9tWxxwve00f-d`* zkF$m*bdY@>w8I<@O0e6Th0v)oXNI? zF=mnxug5zHe|$zIUnrd^$-)%czJHNF%}52Czj^)aZ=aX`U-KDt{%IkaHfDsE?R3mN z8FrsX%RC;gIlFwg)F5CYU1$Xy&tWySIo2?UeSF-o+llGsXVrMkH@QEvsuA?5@3qV6 zU$r?DtuJM9Smlaa+XN@JKY}$=gokxN9xHp2-0Jz@z5M>?+j%o_Y7Nve*agETVuK&i zurVSqHJ;o)+}^Hu^SrREZ`0_ zz@6;p!_&NS3YnWn0o9Blgy z^Jd#RIC9zdzGg5~GWc^OXZrfyx=*b3)wuenCY5&u+hPq9jYiOB!wZ+Lh;}XxR!g!b zoz%JNe`GaKI!<^uwh#ma?HaPw^&uI(*I|0|U84k5Ew@CkQ8!!km9LYi zeA=rdGZPqNtAW+anK6}O9i+=__e)0Nx4V7ZJND^ZBoW*gia3V^zf^{obv7&VJ$bd+ zG)k$Cwz#jYzG32*q@`>=DhUbW4zG1Ygw}W7{8xLbb^MyYeDdfx%`WNE>fAdR4S#>K z$K2+{&66!dBwTKq1%O3dRCSCvn?AF z?@lm_DZ+_0=!e*x+PUv?$CuhJk@#)z5VZM!wtc&c=P*3L#55H{_-yN=cqugXAH0h5 z_1Rx|f{oF6N^oUEHfS{;Hkd1u`@Osz`Wu!q|DrzdR~ocA1))8Uyaoa6RW&ggA(@Ch zVX|DIXz7nyG;H7WUpZ=X59X)Kcsk#Bv5TnWu}MlOA>!L(l9!#Kd+^T+b*gSYk@-$b z@devfjlZLP@KMxj8Hz~3nX#nr$EWS(c04OZBi&WJlQw#Mzw2t{tqnVuv8HTYk+d6Y zyF)T)IfNbgJ%v5Zh3_9Fq2ey#scgIce>U(Y6AaeYUGI?1#6DS!ibWA}cz@@@suBCV z^2>LeGyVB!930OA4Gin25$x30Z*riCfql<#M{b9Ctl#G1r4vPN3rwa~8(T{1hSvvm z%m&i8;*e%mg>wmOyS2@CQrg$+>}#%1q#rKpi{5~{F@*LPw!i64r$6B4p!%QIkqgnH z<|@jdeU`n^x5uD*UW!hdWb6w0B>q|Gry$6x+SAg-fh_IE}pkkzBBQRhDpnrtC^6p!7+x#Upu#Zb9EHu^pQ9covdNK z@m;AoX~2FRCL-QEHL(YA0CFG=3nM0iike=)mnOn*|MJ!E>J##2*|Zl}K?k=ia)Wk?Dd9~Iggoy@ZIA!)y5YXi{>J$6o40s1R_`OZtbbQ}HW_0% z?=9}rg+V-Ty$(nCsM`k@26qGo^D|}S>hE@#HBdDi&8(W%4DF102g`4WKI>p-{%A41 zK9H0qpG`Bjm}t3c-`<$Zt@<(oVjh86f!uf(2FD{_odm8{y2UKN)RfGyllPm9rHrKx zIt&a9UlJ0(zTo{Dlb%jF_rAU}HvyG`f&w!tdh;cHS>87M^i)k(S|pjc8`JyteyvHZ zwdwg%VVG^?9}v<{MGDN>#)iZ=}yTUP?cI8fw?nPt6ZCz5f|0jO5I%$n%kf8Pwg+0zxJYUOAK1K)?pdPF3Q<} zcF57^-cx)6mtFtxOdMglG^sg49v+KDts87`K8@7iZqUI|QX{hrr3bwaT~%8(EG`Ux z{(fPat(39LIe&M`-N*tfmp3D+kyv(#l4_AR&m@@jr?J@A|isVDG^&uf!>Z4qoiuY*>kQ{S!cfoSf zj4Ams+)_lOk>;T1eg@!9i|$XN8zEbXX-N^}$b2W};7T#C*)yKri`e_i$CnjB)Xr|F zTgG1wzjGhk@Q8ib>AhqI3kgBI3Oa#1trX!wU|201z|C-nq(RgryiziYQLZZ^e74qI z0+zSy?-X!S;THPger$l8x=0RI<84i+-?TD@@gH>&<6dSXjxVcyUamx)_iPv!cw3xp z;4>BVYaSJAi_OY&Nob6p%lSL+u?^SZ##@(3e>PU8PgRK_EFD78tw=U}``0ZCxq+w) zK7YMv=_{4&NO{=MW*Mnig*jYSb#{Cl#G;yAFxew~9<@-lNXxnq!yCAQhp1zr3)z3I zpR>S3w=t;C<4iUtq@?TjuXvQ2M}9~t!e#GDyDt;AF}LhdJ%ld~Q&j}{yv}02Cv%#h zz_*y)T-yA>ay+#JF6N|=RR~VT1wyHI^-KE~+i{*;q=f>7=T4>R9oW$)UXK^=S3GoX zRw0SEPNeQAr`+}(jsNpImCtm$qq>lGXUWHuLGOStdxz(;(8N9GO)H(2k9ER9oD=y2 zQ4Nos>_Wwx?_2dgzrS9hkG3%bE*>J!{qtPY(&H03ViO{nXQU6akLBe$k;0bP?||PW zomXNLd{?P*UXoXT{TamCx|G=s<{h><@+L_b|K<7M3jW_5Kye>q>ek`bnXD;jeWbzm zxvPg3!s4XpWh9u*Pt99cy_VPaCRZzT{bxGL0`HE{C`}Z_+mhF=aiswahv8%Gag)d_ ziiDX+iy(bf@eoqRecVqfiVFS}y1GWfC8BmgW!Otg;f4xvuDDx~+rvMf83HaDNf^~5kzwY7q>)L*8e{#POT_wGi+ky~ zs%9?wv&;}uf0Jh=hErbboF7@>t!%-gmDPT{PRscczw4aE#n$NG>x=U(WRIjgF}ko4 z{jUv}`Ei)kbiB_BHn&F?Mx3kBtz?QLcr$9?JLj=@oAR~~qRMoMm&u_Jz&)|6o^xJH zD|`NS%{_9IT{+K<=s$EtJwL4ytb-B6mpZ!b1r-R57#^US6=~DnL4^$|{1bkD0a>zeSb#*9*xCHt(%KS(6A>L3XSh0&qK4v2GJX_pv|*_;NdFyP?A-}y zTL*kY1KieqM3tNzWj(cAjXRL7zCW4Fqa;wFY*c(t4%1d(vqszt;9~^2=JQbKc-f6( zX&2{!%h-Rt@(J;p|6QffQn^7|+3}?&T4*|MvI5R3}nEhFf>0UR1PS@X7Wd zP4JWM=mvTvi!pZ|MN`FNvtN?gQiC(C{j^aha<5)WE5B8~VI+utU0nxs6}%oj7{uk< zla&ppJv6f65#z-f3w793YAmiU%K_n*KeIkbqSgX%L35Qt)q>OO)Ulz?vFmhj#_sW6 z!l>Pb$mFxhx5v3Z;fC6iKTTS}`w|rJI4pnCS*DB9Y*lgTY7$9P>Qo|ftCt7re9_;v zdSN2Yz&6)lq(n1s=bF_`RThMhshynp&=f=GWR!N`M^hsn7qFJvCrX) z0C#H@0$^)D5vPk8$(oeJd$2WzD1?Z&Og6H}IV%0={Ahv3P=1nrlE<7^1&5l)`6VDb z6khAO*V6y`M!sn8@%?)>r-v~tASF6;(H9bsoKaqm{2~~pSYD{QG**@-elxH(*!xN@ zh87>C-pWGh=-`O+!V9;HoXvwH+mN1`jv<8qVwy{5p^YS4UFxlzfI|BjS)a`u?>jm8 zr?CEf%H(Bo9Bb^{O(nPWH}nR6LHSScT;HHYgX4`^&H6pqTA;Edz;-|xU*se#dOdG4 zu11~mGUZ}B+pc@SuZnz7lOCn);K5E0Wr{tbbOJM z&GSTfBtmLqtnbh|tEJ)x6WMp6h#gV?_-=E!hP z!(+Rq36eHGoh^PTsoG@;btxlW-RXMvsU{O_oD89lQ5|BfCK&k9jn`+T^|ou*Zn?%r z20QypwiWe>0tCuAu7U3}CvqZ}2XI9<#<yf`p~fUYe6DMOgw5)uD8;8R##8%}vIEj- zMqr;hHziO}pU2e>>0+Y;^lhES|Oq$7swTv<&?2@RE^Ha zja`lIJRe^AW$lvJh4r88dSt#X#@?DE7DCDRauXI4Z8`B#R3Wn_S8aD z$7}s`|I~bPz=4jA*%sbS5@^Evhr9DsbKQ|WUAoQ3Tg;}zbnaKq?-kOYD06Y;S^b>B zf(C%Hieq1D#V6K#^!N%;!YCrj6h+5>TS_GqdF;X7)VU`CiUe@qKeM8?d z=!sdsMoj2WXv4%*mHieHSv!y8A7?Eh9VLe+oBVPj)`27q5Aqx{8i}&F`nVk3Ng*W(5W%q6s+dB0o2Mo--#o8q(msE5J3vrKF<< zM~611s~^Y78OQdN6%;_E#7a&bWOcJ-q}@;;V&*Pq)Gko2Ml8^(5EA7nOm@I|uzzqQ z-cv?padY>3*MBCsnIOpj8ZnM2vE%1=d~0g!jUTm0lxfhrW`bI}(bw|cYVGX!(RqK` z$ATD1NbT+ft}vsb%qV;N#+XBFjIHM&xzO?T5y2yR!#=iwZh%puo-k*rk_#scXSVH= zjZTnO7JRfeng4bfziqd*!5m-+=Hi8V^2N}BCcr1tCl+?pGcUT|AE<9je-fzI`oRKI z=&G+8EXpFb+h?hqebHp?RqJ#NioD}Yv5yWmyA3Wq}|dXm=g zVv_Q$p_9{-Zi~HkRIp1R@q-J&^!SqpojBig!+%+zF2H@Sch~6GPPh6?zjusmuYWJa zo}ceVB0zE&!oFT7uJdi4fIfux5=ChLoJsZpYeOGQWrM|Uz7D0y1_wuh9;7o7n1Y!< zp~uklp>)y_>tLym1%;=UGXXwA7m?)f&Q(9n_SY*#D_!T`fkw@TZLkL+bmOHuLF@@W zZ-3;Xu$UHj?{}k3)*5SIYoyFyvX5``m?JEnmd@-So+2teBRuVoZ2K0%r?EI4Z^6TB z?$_5|>?L@*)b&~KsTnCDz9!+&-(Zlv{RhOHM7FL-OcDuQ^?a9|#gO~1JH_RTJ4+9g z1)xP-PGrvd(^g6uKfDl$xZ3inWrCb(zD_GnY@K=>Z~7fXMm(48!802r|EBw~Ghwfe zmXi(t{_6en-wPR;t39kgAJr`N>p(heMDGF@>TdIAX!x8c10yIyc&%n=nIB;I>|7wB z`3MGQjNgssPE>CUN*6Gdcbw^aM>Z7OL44cqj`qyv@_8T~gPntm$qqj0lH=h|Aih4C zqAgynb_cW4@3Ap|vBp=JUQttC@6tj#zWhXprO60GkVMOj*LZAJt7SCT`xTy4Vmd>u z7NUjI@&*~+W8Ug|)jF&L>b z_B4o#7a;OLN6ABR=K-)$w+jWkG95JJtG`6a)w?unpDjXFPKTkFtxlTSZnYA7eyx>T zop~L1BmZ&%Uh_BIF#&Kv#N&*+WVV6eo^j`VH_303#m`PI6cxkmmqOI;=MUz*KU=5$ z01;B;TFkF}8Q#BgCq}d6(ICF^m?XNaxvJ=hl86?q4BIlq!0?MPmj7XDKm&>2o=$k} zH3@F_ACvd8w;j-325z!X;g5=-i|+>Djc_90-JJO#sPTT80_?Wv`d^r1C{-L33c6t= z#7~k17)RN+t`}4Ry&Fy8G?AGV90BfqQ`P)uN5cdUtEezue6O19uVf(e@Hlo3m=2Dh zU))-R##}|4@6J@>K9YU}@vZw}g`HgE!n7OrsosoQXO89WnW?GqO7AT_cH1y0jie0p z3j}P$V!hb9CeGNNOvwqHa4#|$&NX&JN8q6-mX1#SOI_nP1xY z9sE5~vHMQ_1{Y$>u#6QLNikn>B{kbRtWSL5EETEfN6-9qi4dT`*e$t4PugF1mysUVHp?!E?H7OVnvbN8^4DWpZ`aMu&TZ+OE=DB4Se<6bYf-oEIkn;v6?t@^~CtJsY`WKjDXJTxb_Qd5(At$`jgU?ckAwPV- z&!@D_zt`uz&DJ=085@ zaX%TEsjmc@i*v2CKFWTRfFLI)nv^OW0AI*~fpi6?ipa+s2&VN-?QE~|mGjv;&!QJ7 zMm?_1WNhx{n{JMCKI7d?X$2f1C;7sL2RY4UnncT3bszL12W#Q48m}_-vilaNSfouD znHn#&wxv7l*;+4Q<)smTV@pY+AUY!j_e{N(y&8nhqEcdKDoEZkn9>hz@t8A;ijb}3 zM-(48fn#NMprvr8y}AV%Kf3m>QBy#+&1HfWLr;(Ia$Hcx5JGo5+e{+Zc1Nuh?k!1g z--9!E`a`pN|6FjdkX(cZsa`uaA$bvS`mrqqu$c*^f}Vg5AP5Q{a* zy~0$b(Xc#CFjdP;PHU&vuIF5Y8yzM~xjIy(C37ohz!e7v-n%$Bw-)LP5T z%|+ID_bw_u8rT=D6Vc%m2h%^Y8dOTZ%jY{49}{~ET*ZptWS!z>rD+JxwH4ftcHrz@ zY*A1Y2|4}6nEaHU=T-f@3q|z|uT}Ap4(JoF?{ogTUeuu17j?mOQPL&lq}}?qHiFx) zb>@uMb+!o{7EM;AGBdfA@7TMx>O4;OrWPF#_8%BZ8P)zpw4iG{x;h-t?*v5mNmlG% zVui@dk$YMx$lVk9BygXx-kRsFM3bTFZw8`7lI<}fE))~o)KU#XBSZmV&R6B*r%ORU zj%Y5ct-l|Jr3tB@*_LngnxzW37mYi(TsI(XGH*g4V3%W&PVdX?Mny|HS}e;*%PhV8 zmP9@*ulSbm2D3=O1RG3|qkA|XszCeTf%Dq`iO6Kcd z{{h=I)A0FhZ-2m>PL0)V`VhJ{{VHTPzJW6-SM4~ECXhzrLO$B#LV>%G(1v=VpeP)2 zYtt5%`4QJ}(TAAca;jKN7&nY=^Wyx%U?l8C(@o_3HF`?hRPLE=<@Ky*$*nC9uV<~e zM#|AFWrN#O&pER*b(rgQQjgOWP0J~Q^KFeP-;5eaA0IaKdAC_Df0BT0b`Mt(ckVQD z+xy%1PR0XZP%5j8?&Ai*HYRIxtgqI?MOW;Pjayeh7oq4LdbqLqd-eAw)e3+;j7l5+ zE$iI)!v43Z@!Z)-7qE(ENc6w;i{6@jQ&XGUGAzy-FF1jM0Mx*i#?f@Dy`UhnB6;}Y zi=09H$gqvd#>fYc=`k5^`4N;KFDGKRFT(%%jpK@ys?+mb!N|M(X$=lnVmYB-3uG2d zL4{qqM-Nc0Twp;UyB~hGQnXD&Xhdm@jEy!;(w#AO{ljXoTCe7D!SCjrcUIq&xH_Of z7Q-Xw#mp9d$b|E}y+o!Ibj2!sQOLy1@}>RufF1cgGW>&?4F#m&w+-> ztgT<}uL4pGJ57~ioTB_IIjr0t3h{DeuP(O3dJ!$#)`Z*TGCiM{Yr*R7{Yz-v`T6Dy2MT*Gi z$gpo9egM#n(3VsourK#d4`e>W(!d!KB+x@D>Li3J{1hFsNV2H0?G`C?i7xYs^n+;R z?MI-Mey^`~_RnCpq8>|J4ecU8Lg#hfA6D5<&dl&_I^FDkXn)(M|Cv^VfjVh3$@a)r z61WNw-+)-8`P<)Cka!E&_~%+1iSTi0s2mM3br9bNmk|9$n+J8?Io3!=Nc*DW(@>qGORq|!_UMt{5>UoR5aiN1RJl|< z&+zjCHgblH{`SgxPW`Ny*BKcXRJOEd@2O?@m%&bKFI2KN>PDT2GT~-k_m+q`fo#M2 z2&Z)V*zZs(?b5d9++dYL%j27m!2=)ug!VzEpaX`ovJAyzPJzodYFQbctCQ<;{zUho zOp&_&H^;ae?BaCUYoGU>v#gshRR{q<7YOgE24_hbt>BEAVLUN$c6QCE*~(PaXp35j zaOHR=Z1?wv2b^WH&s>S75AR{b;PbGLP4qfBImB~d;49Dl1!3+PIZ8_Xi2xUQT@$5W!ubygra zBrfbhZk)PC*I=a`pd~quHRtKh{f^5JYfG0sWWUW0MDI&}{p^jD_tkFB%tv`t#VP1L z?YAL&!&BRQx^u6+W16N3Q~~5+16`C&xD7{gEt7U&k!h6x1@4;C{0c7ETL>Vd> zYVva7;|n7fPy@eQ!zU$&MjP^=!Ld5+-Mw#PgZGPm773I$CcE9IpywkWojddsF+H~6 z%0t9~v4-Qv_nHJ}2U{fWd54NcVQf&0lgKBs#&g`qSH9tgrxE4LU!6V@^}2Y3LnRvF z?B};J*V;T99o&oRbv>>f92Nr4Y-bR;=Afja%#DyTvfsTt32;%u<02$Tei{)_&3zd` zW0R{k*mG6h7DD)QU9NmJPcST*<6eY9EjxC!_hTxX9d{&Xsa84LFGR^mh<*6&g*YK~ zpyXp*)o%>YI^ek_ms&(6rGKod;zCA7>zMzd2g0{fCx6n~`amh_UyLuKKuXXQ#|NDV zaj#1Qr3?|3=MT6j@>+${Z?7Gmr10Zss{_>Wyw)aC{q|Po<+xDqsy)`$g2vobDeUM_ z{;=|RBCp}KegEQ{VWy6wbknwrNJ@W^av{@ixl(nVz&%EI5GYR#A`e_}UVveQB!ETg zt2`|9E0=RQJA41+mm&pov=wt9(-#Xt{myWI3e+CzUPSlQiSiFGsY5GP%O&%F&yoYV z*8bCG6d-5epH8POHjaAfUy-kzM^K$ZPc6lWwuKW}3TvU)ro%qAtlP<7I7RtHg_4P| zG;2Q&rKP(Q-ORs17H9ax`3PWdFT!bt|HMP4&;_4A7izbrQm~o{#FkH3bhfrmTgT+m z43~eDdcDhMmf=Z&*>;8F-(m93>(*nVoj=WUIuo|^+wY8_Ehr%;9-a`@sIh`4Wd79i z>*b567+~b&cKnC5HnMcpjKu|@goP#-~IGk#WRNr z>m>;tL6;$eFjbE;iBuj;pM?qp|1ka0;=y{x_O9FFE9|WZ=Lmq@H=FHFlS6*E4ZU=D z4l>i|F#ohM1q8viKRNzZGognB0pb|ez#o^G@U>i(u{U0B&Q1Po=gT1|vF5QB_J!03 zwh$R&{%ijXo5EoIw^7~B_`9Rc!Skg7TvR8}lD59P^#k589)=1WpUhi+z5r*K6t4mv z@_CI-q%UmlB)JAc8{1#`yMEzErT(CPkEqKr)2*N#-92kkk%#^2@fLlRo_F?ex5E~g znLZKcjP4gFF|=ghg_>M{a)LmHx&l`tW%y{U7e=o@y1Qin5`{G7A{`OOnDttmb24GLY$JQe-h4TshpKgmCEo;lTi6pQ?;6=Z)4N_mD@hfmFtzw3oDvt~FS$MbVE5WhsKt?4 zC?MdGthtDma_DDKS2B#xsyjQ<9!%OS%KX_%e2-+x5M&rH4CWnuvSSt%b%lv&}YR+2EH5MS9sg30Qr(eYifUA%1qvenHb% zF&q!z1i<_gCMnZxhpBmqvWwDuFDz8)dO%0v_=s&N(;2objS+*VDGEM>0=9ymYW}FJ z5c4AxGBPs$HWTbRc1{269ZPP;r<}^)mI6fSuUnRaghQU7v{)k&xX~#0XXURnlf6^eeJ=6i{-lR0~C%q4RsGHg*JKUap#Ts$kHo zqqVF%LdDFh_ZU&kC0ACmTa6CgPM5MRhoA=3PhX*j=?@NZa;DwJ@Ba0@bpBWETR4cc``4$(N`KWa& zARwnLr<`#sL+9)RQ*E%F+d z26qo66ciJ==U^d&WdC0C6uf5MZe`@rY=a|4+59}X-tf>L4L4AunVi^{2SxeY;JNEJ zs%QSiS7$j)jGyO0e&Xf3#hocxWu}U|Sj7Q6xY|q>{Hq^&3t;t{m=Khtq~O^G{Adck z><{&P{+(ap0bLMg7b|-@AA@N8;e+fm2h1|=r!R%lh3 zqU^65esaO}hLz!8y%AQ%JUA_A4N9Q6&+tmw%%CRe;P8oxx`Ik9uVv9&BqSE+>sTqZ zYJ9)Ww~ceM$jK|a9x>Px5JsnbdUMf^fzhj|7*sIEdww$cOduOX&9c`=3zY-}c+Uj@ zQL_X8GaqSBY zRaREufxoutPtqzNH-oBawK|dA#q6_}u1hGB z8fvthbcpYelIzY!ipH3}4?ln5-Ns2LnXg1^1}aHW2@>Ced;ECFIbm?IQzQ9D+}thb zcu00|T83934Pv0hvyKbV+IEToE2fg9Vdj0wm<}k)>p}dV^cC{5HC-qXTV+N{O6GZ z6HV~DAY45ECh>&)S4pjlop?_){ZG5!vJf#lYc}id9(q9$YQB}_D9}MQ#vd{6KCy(V zp5C@_JPr<=X5gJdh)7HLc)scg2>a+*?^{DdKk6|97Avp(+Y~0hKABiKDS%31`}<;I z?cS({(7t|UtvA7kUnPRG(FGY9C!akN-~9dU#2CIQp>X1w5N@n%B&Q~KL+&td%o72SGfJacVj-SCtWkjLo8 zNCl0SZidrv`CH1EX)UlQiC9z99=!yGkn<&NIhvd|GG|<@ejdYH_q>^PK;^bT08$}zaC?QFuK76rUM+b~&QP3Qd;BKHR-sD;V|v#bD?{znH+Iqf;#ZBqh5-6E1#r ze_Fu9U(cTp9be1MHQ2>uU+k{6=njTo6f=7I7uk!J=8~>H-B=ssZ*t>q^H{xa*!2)R z#r1Cnz3<4HH7N)|;&g-kF%v-g!V_Y_R0|6KfU6@RbVGU#zF)KH3Uff%SzbY2(o-b{ z1X&CG!p2I*3dVv(y|n7vua1Vp6F7NTbrVPv&1w$7^S!4`xj14{b@Zo~PC+iBS6D9! z3zb;n-|T-Q{3KwVI$9ar+x;g45^e#ghXPmNO{?d#xO=y9QbwCV0uUB|kC(c`WdXt9 zJoQ;^gx$#`XIx_3#;M}o^oNpfQbKs7lew0UCrUfnJx=+}YqgLAk>5=v_GhZGq&#JS zQ;ZrOV*4+p5ghZ7%MZkrIv{;7)H#YrdRAr(QsB*OO znK1AL@Tb@SdwVUvp^gJ>DO0laS>Y9^AZT{>tKgLv?)R0Q2nb^3ii=iKG{fy#y+Joy2U})wA3K=}%W5xDRFGWPo z8YerJzPR9Td5c7y&Nuu!3GDn}Xy@Okw^8j@ie~fnFU73Ff&!PM*aeO9K$%kB*YHG> zZ1;b9MZL7l#7c5CH-C9}cP1R3j0AE${t0cdX_(0_Y*>`<9e4&*3Hl4r1}CEc!vPpO zV&*nJ`V*gZ+QKk{aLE2xjLMz=)VjK&u3iMq?FqXlmjy9PUmwhaa;ONt=E6TB1=c0y z*!~pCGqgJwO7&Qs_F`FD3@`cVd_2g3u!T|jyPtsW7x2#p{`+i6h)k)>J2fIUN`d#C z6uh*1-Qmq>(v{y=_TVnv_2~>lq$HeNr2qEJ>)zqs4wq1pm62$ArpGK?Q(g*prxd^o zU8Ev1s`3E$4DjbdE&j_Oi+(NJ=7?c>w!>*2OAL`JCHBxL<=PYm^azG^{XHTyKTy-l zEUspFORS^OJlzdPM(E+Skr8*UGv9-MSPlE1-N4g?OA)A1XVsYk41u-tt(4iSmLkE3^LDn&vb4ugp)pfS-ycB%Bu_DaFh5V__k4Jm1Lzn3l?_C?`7~ zKUhq>bCW_298AZ~Yi<3xXSY9NqpUT|g^A$pjaoZyb?)#1Op`nkz$9ELNra5^M9AXx zqReOL#KyyT-hVp zbd*yS2!P5G*6!}HJx6B0P-$djB+Asr-4&#zQ6f^qpM>|fF2QrG_?!9pF52L%E%92# z7)CiGhR0!#z7Itp;;}ek0U?3j0!dQ1ImC1>*ZDa5SR?uAcnf=**yQo1Q+keiCdUHy zbrGE1JW`+`{a=S&YA`}43;rjr>%I6b$DOzvM8wVuhP!%=Bf;O{e24z^cky{Fgk{1y zV)T8w+P&|nylC8z{Y$p2lxk~_wqx8^?+wxL8+?5arYH4J@WRnya}>j++Ci&~iV`Qk zW0j_-c+RImwQ=jOM*q*>9pK<}@7#Y}u?#nU4y5@To5no`$AH0cv!ltK3-^V!{|VpJ zLk)KJmtMO(o}RZtCbw4$wr2;0N7oDdpw}JHHvfB$nBxo$ksh73ujFu>a)99T&wSO- zldkIi--+TQL_LmowOxDJ;_w@t*lPNHL;&h06-lku&c!n3#}T-c zI{dSblzIGVX&pzIp3GGu+3ZXOh^+KTM!jxx5;*O|WnyKC;fMQ6JkReHBH=|_{S2YY zX48`biDdAkvOjkK03^gR3I$RnOZ6f6iH__-o2&PYyV19vm9w#8Wpy1aK{T|0Bb8qx z##2+P=$0gAw^4vpqfh>`WIS$~?l%$ULm(uk@E!r3&A`md9aUqBmwV#6jPa6+PvKAg zzKC8mYBLU{uX?d`bB9=oUK^zxiVvs{)KrUu@kN!!OKqhxetUc=QP?B3MvE1s0#xqw z8-oMQMB$GQ7V*ai1J&;OxtV$HLzIBU@oJ_{y!5B+A2$=YzaScnR;rE(36_5;b^ehM zgpTPf5~AZjWVq~&-{`yuQ~Xwm)B`_(fJX;s30KPGx$Ebxt7mR*vT`q?|77#YS5~6d za+9#hM!gSC4h2zoaU$tfEj6P!DRaQd{~Rh4dfp09O_A#zwx|=d2v8P`qt6OnSLiGzR^!#q%5u zt=H&!AUJqHvQ#~%q(!GZUx~@U#*|SRh>9~b9M#y^*qsN2emy)qK*Sl?IeDtZW2+zr z_~>=BG{hP4O;Hp_Ea!JfO`HHS{@APHG4fG4Ir&Gj5>k$dTBU(nrSR){JoF&^+O_o# zo-m0Yy;@a#iKKS=bRCJQqTAe38g69%8(i5vrDe(#)YRVC9d?Bd@O}H=Ap4F}Y%!{S zf#&N4DY5X={r3P}c5@%Z6LV3x*yGb1Czkj=GGCP~N~uu})t+z%JA21C`LqByWhaSF zHhw{&nICW!vFhaV#Pd_DTH%TNC4zz!BU(Oq`J5c_Y-r%PuoqTKNR7@6vs32rxl`g ziV#E=H+zgO@op?5O=TScB95mepO(d&kP2#ACyU0*jgfS?LG}U(Y2{LkbRF@QT2alfAk9se-4TcEj0zA3zg}o3D5D~pB7lY^MIU7FY~)2i=(61 zBKtn}JsHV=0F3;J@@ePr@M*AaMFdZ1I)C>+^^>IK@n)_5;t-)lzc!8KEe^XHuQ_Wp zE=x)cU`C8`q1q5cKx5bBeNfP6*`yw||2P0XO^alpQLFtRBs;$dhGoFvTL-a+q~4e2 zEQ~hnM?lUwOcc9{J6$`zMg$kJ$}*We6zZN{HuZzCH+M%AE(KY9j9qI)$;v z)gfF;7ySlpwCL$hPt^&PpZ&sutYw~-Be!EOo)qmCR#cSpz`ZYg&5D&D`iBrmHv&`P zbb4stYCOb%+1I70ZlR#gs|Q=nJ0MRYPo);}gFIfID;$+pO>nP9F7Hwvtce`*$7K^i z;`p6@!1bDCj2o=HRwPO-Po*)--fC{W2_o;j<{b>iY}&KI;EE#|7cSwg}NPL{xXpI+dpN}y9ZtBd!Z^YzR+U{Q&H3loG9a*@p>1LX6b z0RhY1O%uYRhB6U$`ur1ge?I5)pBn`2ZCz1~E zS)jOZzd|An?wjIDe@*rdvO^+$AcwtPkMM&^ED>tF2lWvx%w{4N8f)5`K}0I5s+S&1 zQGhf;h7czm*7s@yEl||j9-#O-DiaN3`Y&P|aQTWu%8<_S<86EekwEZ@H~IXZy!qjsw$5k>sXqmXy+PC`+{_a%+FN6U-i!uzI5dLX$FbQBJx_zWmg zhhc%`#y~0W0M6`9jptLS*(f5Z)9~$nvfj(rg!!gpFmF%?4=NB@K<0te5$6GASHD3x zHccv*T%|~o7007SwuM~UG$h>f-;ZMoSwLtqmexbqCtyj*uKkXOy!1iKWcdm*saoqr zES7=VTI8X|ldF+c^;2`^*%gA&KAjl>zb9dKAR%U^OQ6 zuNDyXknDoi^-m?!=M;DCtsc1J#VmXLw_XkCC|{pGv~PbX_TK(Jks-!cxm6Y9V%B(n zRyDCoE@}UJV?*e~f@9`CV#X^~?YNf1i)oQnn2dD9UIoNJUS(sGFi*UHZ+kx`>iuV* zlG^HtDL0$-nAoT5XGzZ-#^*hDZ_3V$?1#KFZc0p(rlfOGG*4MBbrkw2n+oh;7^CfX zWr%Dr4)|^KFI<`KVy?o>=17Y~Bf>(t1c>p6ZA)LeYm-0Ha{JZhPT7c}Cd9xr+fww; zg7LmSRbkk%=$P=e@LPX@EB84TIuM!NwQz*@HM--8${||rhiC2R1L2%)_a!FdpN8&J zjz~@vOQQF%C_M&BKPBd8M{I{OtgU6Ty8sDIW%P8V zy5BE%d)f9Jel^4&p7g zK+D=u<*5tk7S7P!a4-15`?P|98pUo>Q{JmUJZHwz7YO^VVF!6KiHB{=!X2KA^qk#{ zp*O!I(M?r{>nkl*Gl8pO?gg)_Th&e^t9svD;A|Q8V^EOPbnC zSWlkf?ErnhpBVpODXzOW+V(10;l;->eOjb(T@}6{i(Tqc+Lei;-PSAp-zlEDpsxesQ7fT-`pWTL-SShA%778 zLUXi87{Vg#*U_-4id}0;XvT1pr51OJl4IFv^&}#Ee(GNyui>#% zUt}ga!G9{f7&V)Y;+eG?9NW%3?NcBID#!<0%BeS8DGE@s+r&6qQVROjpp$k`l|nUn zbr4$OXJW|_rnHaFSp9H$hXe!F38d=_tv=He5hHj^7a&S?|aeuI=&O?jv8doVMrf&*SWN4kaWs^z>6x zKrrw}1bsN^s+ir1`Va0{r*p`A`lh4i{A*qZTHinCxn%p~p#4+iV_VT>7<(*qK_sr8 zaeZ?#UNjSDqiDxZ_D*LV=29nMU?Lc`1%hK*=Ue&eH;~zzM_YF}6}NlgU&vpzqJM{O zxw{S8J)s*L5hmdmMRP@4wo^epsGoMSJ>wC3;_T!xd^ZWr;s%2MN|v3h){2CH-nd@< zY?VExkDvrBhMwbrO_|A!z+oQOeshIc2C`bUrb!R!x>JKp-aXX4a`OL{ut{8?t6Yaf@A1zIH4UsIIbQ0Mlh9GBMdh zo=|5AFVEev{tAYkq{JyZ>d}ZTK85#UA66g0r?wN!*3Om+y8R-?IAy&#xV* zn3XU$#fK!gudr>=3d8zZpY=Nha8#H2dA-=Xz*|JhC18!W-n*#8e)<#;T^#d4*k?Uj z$G2ez;ly*N^n>;YZ}1Fte)8*~Lib?%_N&63!c><|eej)YDsySn?9XDQ zNxSzvLyrA64MvLkJGJHq=i1XPRP@z%<`Zksr$2P`oA4qvpnTed+0-_?93HTIzDTLU zacZDq;GUVWJ)eC>h7KrCA<2GX-W)H(gj9TXa;_!1_oG}j(^==INT|DTs9ww8ZOqo! z)pQCwNUd5J>8@;k5xQe3e{b}b`y^Xvxalef0&Dv^V1g9uXEIM zYUU1&g6(BqRz~APnCt?|z_eRyBYFy8FN2BFiMb^DOf0n}Vhf(3XH(FG{o)s#8vzt( zKRAWnh26M|75}Go*wzqyqG!-tw$QhUAChbAY^kPq%q?(KYvvN5ei^8HfMS@%=AqOm zNJAlAWOcICa<|!X*boC1^ zc*RYD;@LdOgFV?br|5Ln?s6trL~C{GA$>)4OR-X>Ceu-NyjNiW0t*|=+Jn9u)0*b01qrwWi?!NcC@-V zSSSB{uc zr6%ZDI20)IoYn%8759=?WPXwbvFvMIq$xf$g2pom?${g7&GWCMud47Q`X?>WmpoN4 z7=0)F9*g^d?m+lcEjm&~k1_WdJ8n#zUCT^ACRtgSm_|VrUh`1j5uPWilQ^7pjq*Xq zi1PLMK0j%`ni(Z)eQkUGXhg!04$-*oy8%*=F+#_6uD5q1}=C z5{)^X%BJfJ%bDd_$RD=u0xnyO2RjrJ=WP2nxeYsVa1$mk^FQ&InZ3au*(!Qn z;Y`O|a1fGo6LMU0sRe8poyFlMqx`w?No$i|>M(Q51sLo`bM>{6LtTT}-m_i{VXYcQ z4cvFh1D2vAL75{40ST}8RLj>q6Ovw8awo7BSeQu`eb(MOJz`!slH#_w5E-z<+Na;r z^5i&QJXX4$0WpK)9z+8t>qvxj#<}&x7sSSkHHx&nXpY4r#z3s~p(k{W-@bYrtw9Au ziFQeU4CdhtA*%n@Wkd>i2YG>?w9AfM30Dd|fI zN}AF&Id$R|6x~aCb=iu-{KmaFiArUFcyek$VLB8pEl75(n z&m|j&JCdzIn4OFpOdXgX!InC>2(b5^$+rMG$$P7tSOSy%^E3zl0=?ttK z9G!CuJI1y!k%fVX9Pf@Bu@vt}o=YC?H7S`Yl#bAjH!5OBsVzQ_yl9FXS7EqGQyPWa zG8xi+CSI{|FSOW5oiX=?7sp#}ZV|$OVJNFgAu854e%E|G)@RXCXytu3i<$aqhH}7^cXp;Pd{nx>J=(Zw`rI8)1ZJn9o
h zD3%(KVFcVJkK*_uZQYz?>dlU7SX))sftRdkXPTcr5@yN`@B8aphA6~VaPk(heFpL( zE6Vi8Zv%G-Vv?xtUAxS;49`W*d|Clii;jL3`Vk(=3+|QWX)<#uj%I}`WO3$yPrPQD zX0_;nhw88rOLA&-K5Ex)rKt7?S{)SobZ54ob8GXAFy`;VtlX<-W+VVmmFQgTu J$|MZ@{|840_-gPs!Dng$XPoG1R7u%Fo>8lV38w9F!&%i5ndDPp>pmzed;rTx7>CI>I)URI_ zTQOXwmLVr+{FxEamovo2$EQYDU1*qJ@7`Lc>zQNcd{3U_v&&5bWnFoavos{^rk_~I z1_J{hpSd1);aOXorcG1ETy1BY%v{IYq`8H-$sg^tCEeY6MDp^}(>DiEX`}uJsS;{y zUH5q0mU7qEo$~aF$b^+!S`MDBnys7{G8n&oP-K-Is- zK#=_YqoGPmRdv(VLOUznl+2Z5l6Bj$h$BVa{PX(b7(cyhxzR}Vp zQO`Mv024vpWz}VgwT0ipT1O$QqTW7?+V5uEWPod+d)`8r#wrIZQsR>;QFnY#{8k3* z`#&l{6xzFIeo)0%qT-w7EZ_EsFl&&2^H{T|QaZ)UCK!xK?#9InER1)w5b4mGy1HcX zrOz>3exmMCb!oCD?xOs|r{6ku7=OSrX5=q{H{La9tk3Ra>@gx$A9694iaiT{30!{f z#^|spR3RghY5nq_uOPoT!bT51F!q>qUqQjtVLS^AP&rd%|AEA{zXO6vN|iCfv-Hpy z=x?tFNy0WMXvfi3aa0qw&tXGF_3tP$n_hY>?hP%M%)7;El_%_o{KFfkZ9axK9JmDD z_pNT2!2S(X7+cM6gaMK%qs9ydPs&!f+~eXcTINjaV<<-EketkOMP+}-Qb#q=VOK^= zeL2&fV%kgZBBH_*@cMeJyFZQfq@X3_Nrrw@e~0EjugH%Hbt0Qgxc!w z_iUF9P4O^fbKD&hJz}UQ$xWKSKLj&6dg9f(FQUX$>-rgcZbQ8P-7#%oioXhvj>GG9 z52AL|4!Rj#BZYBv`A&ZaP?I-|>iXuQxlm0ggPXkfyY*4@SKtn=47Cs)xM0#;^QM95 zImpQ*`Q}j(m0TEj7yc}bXie_3!yElS{G}bD_I0}%Q1s9Pqi*fKp-UNgB-EFEa`V!2 z(yHEM_AdaQ8@`H!!8PL1l&CsYI}Tn`y+m|IG4|RK?LX&xK5i8W+qJ0dhx!=kJreEr z6Ik$vmOenb`X^KWnH%!l>`gTku2GEMM_K5Ia$$y{sPx3j5ysnJJU9LhVcYW-u;Y?p zM+6=Ehoi5cSd!d|&`Dg!+TE+<{nd$gdKfp2S?36u0Z?(K;YuGV?u{%8SEFsLTGs}v zM@KAWahh{32%)r`sN|kTnIJL#xauo6*xW2(mA7cCg`$Kot z_&fqv-QgOyS?B(yilEFXO#aGZK$gcr4x;05usrxGl5a#=ijY#(@10?DN))s#xp+;Sqt zt;B1B!YOAk`DqIQnyn#Ro&g#3oDB(3mxQ~x%7h{ikclK&eX)$)#5A`n{KacNkw8daE zuEHH+x1s=R{opjWiF(U{C`22g0%8r=3mVZCT(3}<7c3^d};APWfhfXI_flhzN!yMcZChRU7#YkR31bP+L6njV}TTiGewDi zzWGF16(&$%PUqUrWxgQG$ve(ip=6!2BF@L}WDNKoG0p#|$1oVX;|15Ch58#B4H@yk zy2DqqN3`H=?94YEn#=nfwoLz1XlPK6F{9iI6>6@mszcWcV!o@5q@yPJ%zJdKspzkn z$pVGhoU~f~!9z^iTGI4CEpW0Irj+&BsQYyKe`TZ!%UvzJN(>2f4hiv& z-f#tzkvy{w;1W?+*b$j%pYF2K??87|>kL%N z{GVYxs?pwnF(?`T-l9ES#S8t&Hcw>&g;jff<78 zu783ZmJ83qpbS7w=8LWDIhO5U>@n`XgtWh>NiT|vO4uTWZep<2`;vEp4lwX~4=oi4 z_2gA9F5byEoHJ6)ga91*9yqth4V6=i@W=>*+ARl+Ygqes(KEsm7rUe!1FQNHjmlSz zYFPzsqH61N&kBLU16Obs8T5T$i5M z&?U}`PM!6fIjTdyO6C6C3e;Rr()39<=2)*gX=V=h#*|CuIwA&LYG~9YR=l;Z1kJ0B zamnH1f0FZF;BwW0wCLaaNMdgZxgL&`O7sogHmsskAcZEWALM;s&BEBJ6B~!@ZWd#o zp8gG7h=r2Z+e5AHLllbZTO-2?-|j{<=M{Jsr+boH*7qIs3Lg;fjL;0Vx>3uI|6J~Y z5+giR{h$w4Upone!!;;_?_t^Db*pg#=)rOE{)E;4p!!eZ4~8c5A>xF&e=x$o^F#@c zRVn8uVyog&^nV4mE1B#g72pyra+3Yd=vx1MHSrkd@8^gaz4pQH?-1%OcFxAE_cAv1 z2EyZg{$|s%LSndL9TSA9#V^5%KKiq4{6TDG&OhiikB?m4m+(-Yo#g@$adQBO8z>9O z>2C{UCPEhFwl0f49qC#iz{X56aqU+8;q}h zkge<+8j0}z0Yh5G;uX3jP%QCx)Vr(r`d%jBExRo_HBnoU!Z!87Hi^DVB#_sfni(?n z&brPU&o&1)=a8Ki9{NPoch!@GBhhCbfDB+(d zA5}Z!;q8CUG0>ZzFN#wtEcVi@?CX#i+3p+igHnwCD*_)PKZ7{^9cm1PH?XFPS|d_^@w9(c^E|OSORW9)AOu^5o(vYwO+!Fo1g#X5~DaacXw{TmOa1`0e^@EKLoW?%_=G~qsi=Q7+9+BuIe+D{v ze{psWxM`$KJ12C*+4-vb{!B#F`oWqqy)aOc)OI%7 z17^nNA2P`gxk?8FmII0<4sWbCMXsNNdn1bCR`(f))qmNg`4?9;AuV& z!V)V{(&z%`6dTmB)& zUlaB*L#|w0HRGp0VN#giXIDwX{RI|~@U~IZSgc#FF7413Ab?d$Uj-fsRD7*Pc={Vj^7-=7lE=MKSggHojH-kLK z;{;5%x`!Q?ekey8n3;K}AR+6`pR(Bfj|OcI_C6O3)%xf4ID_-Zqjs~Z{C+&gbC`6& zOxYF!b`dIYjpx-%M8TAYy~g3jQCG5eW%rdl7dXe zWiRo*r6kOEciCN%Q02S??8IvtUPjjrni5lk3I8^l0DAe?L&kxZ}TAi6N#bVGJfd*#9#sCQ9s^n8+FBtWGVf@#Br!y>0iS^3@m}Z zLbbY2m;Y6MM4j-g9bBWb`s^#=a8$ZmMoH{ez0mW2HZF^}+jc~Y@=-r{fmVW^qP8-H zXERW_YA}1uC=UQSA3$vZ?%D0U+Sv?X(QTD~X^D*9A`*tQFA$z3hnjMYPP1smZUr6Q z5XBt{7AWcQ13eVhQ(e-v;=c(OV(=N@2tc~D2e4xlKs1hZ5JOxGj8KgG6KK6scWvL$ zaT7ThRN(b-u}s-s0=bgcH?)chRr9&mO%7G26XZp0r2%euV1n%56S?tcD1@Zw^NQ}$ zN0^j)ckypmxCZJwi&#}J%yjbe)4wCAFGbhnV*nnj-Yv-b_;0)fy*8N20y_^BX+_`3Y?2$m*rx?fCn=8Wp2UWZIlahw1W$`5FpmJ|0q`d6?l{zecdbnV zTAWL>tZ()}5JV_Kzw!rQMuWFBs;|NIS<pQg2 zOb*%|DYS4MihL!Uw0{HW2apM$_4ZCa{?1>QtZWVB8DonKUeWPdu7WH*6|$GVHG zLIZqWs<7@W%=9L}EDLaNA~_qc^BQ%Nz}t-bhGt~jC^A`GYt2Nj5>p@b9k0Kzk={*h zN|Q9?;^ML>mDwCZ($Ub(#I0c#OUwc&L0jchG#pb}r5^Sg8FvG8=r{(g&$VJ;VshxT zGD$?n-Pql;Yap#16=S@VC{R?-Bu{Jss>nFUGtZ9MTMMK*y<^efiSpLyPD7nsU%+uZ=s#Q&K0}6B>FEY~Ss!9{o(|`>1s{rTgd-{qjg zIFBb~n@#jWg=f!09bw3#hVw0qan~0v?Jo564GkS90azu!r%%u%>q@6g-de7<-16s- zc7?64udCUQkMSTu*O2#js>$2CgThyT+TTD~B)T;V+oS`%&&0yzcaH_Qw&UH>IFr7! z+}L?tN=yt8MytV*+T@K9*8{}IGa04+eVXF?ewqZKegL(lh*KaQuc+siQ+pZ8P#;s+ zAh1w()gcb}eKC9zC?>b9BS2>rD}FGMwDzPwFD80zcz2xvOr-ME8#}kkB~Y`BaCRKi76F9%}9R(`e2GngozZu%jxz3I{OcfXW~cpFz`W!+Anu-YOLqfH?zBM@z>O zgl(cM|5&bW-pP~|h@l$`Qo9>j00Eff+V1-}SyhS_Gb|qM@i$vy83voG)-x+oeM2p> zd$$284C|{LwhujXsvJvM6hrrpeZeJWmPkC+HKjw^m!O44!T=J`qhXz6 z@9jLSqUVwmgM(h3m-T>V<)4LQMn}GkGEg!wkZ(c`yb}LD<8pSoo-l);L}3=dZH6!? zS$9?xCtSmn*{9!LC?Nq0OrQSbbv&AfL8uRxz>oc1RJzsk{47k`HJ5yI)Bix`Vv4wN zGlk&#LNYj$j^$o)J|+U|fk9(YV@beFHRujLeuw_U&3Yodrb&tz0%nujE#=k)ZS>(4 z5$OiXu-y%;B597td}hzdk?oAbcnPRG&})aq9k#w5DBgcX{xz-sY)D9Q;@g6eGKDm+ zSqPVS(Gi6__(WS9zV*%?3JvE(3?T9r zI(}vYa4}PCXKr?}eb}+znAOi|E3(Ec;#Ew)zIqa$*!7Q|6NNf@dRcPOSF&HFh^wQr z;91+h11*gF9yM4tGk_2Lu1xm?`pCeg^ZS&QRxl9&2~xyMoxP#_P*?h!U8^Hzq_jl& zm^XV~uY5L?U!~rfm|qJO5a?YnwpiFj8KDwO9yn*B6VOM;`^hr?RpWmqr3}lWm;dmz z{hI{3h4;oyd|xV4-XvuG-IwXd%qxxyN=nKLQOMG5=d42ezuPu+uh$5$%<+aD;B%Op z?QMA1z>|sy3W}J;O=7`1)rSh^k^c_sc3MTc7`dqx0JG(Np&(9977k}+C;03>4dh;WscGgPkbG3R|aWP4h z6^<(=;$!o9!QddBW*t0`BRJkqZ+12&R#rD0mSkPK({-iaEE{lCgL*K*Gl~wAD>v&P$z3oL7bkH+g+!zyP}3XX@O0a?+H=wudGV}ffO1JqpHjf zX|<)-1HNwe^Gl0#=MnmknN8W?6AfV^$V6>5*uq2IVK!c9o<%^dHUVPG-+JjJ2|O|1 zHxbzC2()ld07gL<#k|UffF4KO={jl4mzI4If-XqT60AdJ3GDA(9B;e$k2%nS`0whdhybIByp1Dk5q2Pr3%f11?a3}LWEw@{8pQ49Y=u(YO$oJd?;5>*$Q?Aou2I`(M*tWb;R zOy+yk&VDS%izYPYNq@S`fxTrk%mcCHM&Lo$xeMjhFJ+^AjacT4#2`fdvI6Z3WRMYs za=_&1V3M31?i=F^Yy^>UU|kQ+*AYN~MIuvhG8%Af1)cr$%9fB27T2_Mlz4$SZryHz z_8`tGfVr=S{^?mQvVdw<__~|ne;*3i76NgnHU;+asM(uNmv76+xjf$HT4_GT8iwai}VEnT}k?+w26_ z*(aO7XZQpJFx$KI4D@zA5p+Y9k3#6R({DoU@P!FOR9Df_FG%^{nN+|6!72|Sp#?3T zfJMaRTHEk7Dw3w*i;~#)d!Kx8Y|0Q;?6mZE?;U1N^?Tn@J8{lh_8`A6aPw6}WW-Ur zZENU7*dFfM!EC#D?|T(0cgal+X3yW>B3?%C9C{iX=sd_rynXxqB0oOdmQu&1Vmc^* z&=ECgnTf2oIRlg=#qLS9&j5gwN&QNkIXkQI_WkU;^Rz>W@$sstPylMJGRX{07|1*Q zHtNpJ%_(r-oaOueaD1%o-&ueku55$v0JPp~wf->8=^Iys`c9w~?EjOg$D0ZJaGbpV z@hremh&BfsykGrLhe4Ao9zH8M?a7E|)V_CS4^@p406(_V_sM_-J5spA4mo=g!l z1fXTshzjtpnPAdykCpDGG^vDY*SwvTfp;UROe^i*3Vsd&&>kjcpKJs~dCBLnrQk>* zBMm*hR~5_!Is8UUHxOLBfq_jn+8LPB`mNKWoixx4Iiu0(XBntKC z&iuf7@4Hx^9u%qZA1P{Dx12o-`#li`lZA)+qy{$mL^Hn2uc_fU=tv$wrL71jVr75K zjT6X@${%-fu0d6q!(do0`=_5n?Hr>dFI+|2FiYYk)%{%FN6T#+Fb6IcpwNxO)ySFU z!2^lg`osO0q8--|Z|?(-Zcc#|G0nUYrx!yR0X~=taB1@X@}R#{@2&P|Z+{Z)XVQgU zqhZEW5)tFAy4bLbz3NsI69GRM`fJL4%-v$aPgSv5L5A$9SM-! z)UMff!@7bB`Ox>Qt9QD>0}tZr(tJzk8xKnYQe_*z%e9^Wk-t9xhaI;thybTJ?yi=A z{FgeoK~59cY`~k0`yUio^xtSX=oxk_8aFiV4M$aF0f|9${a|Ukwck>% zEYIGsFW?wE)_me)4{MSRS^zc2b_+~5`C*jtmCDL2u5{`{_!4vXZ|Xt`bd}-Zl$@B_ zV1SiPr~BSnZaTzg>Sptm4pzg%az_H{6Fc9~`~sLQkkTzdCL%AhIl8srbFx@usAsk; zY;4hu`_fu;+H1@&!=dO>%cSbP6)m;@7`xo3dFp@SjY$^ogg4dXt8S)y-H21vILr?g|HEO|nF*5A)Y9@SzLHwiK_CiR z%^oQ{4>fZRIaxp2v>SMzL%eW?Ls~@dg+rquacR9)$*s=jrsc5)6V_)`Hk-WnI+mHxQ7T5OleG~ol$Ccfy_hQroFo`gkpQRJLkZD zL2qVd3=>~@hj}6B`L6Yb&~o1>_ueF)0vZ5xEG#9Hlas`7<{qhYH$f@{ zF$lv@K}gy{K8MX!s%a*0wzRe&@H0vLBF_Vb6rSuYzMyjJ{P4^$u`&;qWX{MsEE%z|Eaw>x5Ua|rZXdU zPBfvrFac*;H1I`c7CywXbi$CuZiK z5r9Uit(thJN nJFL8;1`CcluC4W%5eh6Ygr;=m$F63`WuW>Z-=H}!2F_n^5w~9y zPQtns%*q~bU5A6r9;re=IGL-N080H3Gj>f}Ygp`Qe(zOp-(9< z?plP@?q*`^`wKxX*ykXo{nigJ(`9c z?f#3r$0whuh_nd%vwz1IAD{SUU=p@_wE6gMtPlEpEcsz#Td>>0;2XujK!?EXZ4BxD zSRph8KlFpyU_B12p1AiEllA<}@yTYVtf|oDt@__;SgCD;n2n>qLRiS^|zDdWEExix0axhzBtI~qrJq{-xZIF1z$R-b6L?C(>O5UQg#$T{|) z!|4D&Gtw|ig;gTwDz+7ry$s?-PEI_za&iIZK13wUMm6;QdB(4BFr?+6OT9mAk=WHC z!;)Jkj)zaaaB?B`6sdT*g@3{w4kHXP|BW_MAW7&(P4(`87E=X_@T4I;dLs$6G={o3on!{;2|!u#Il@dbBxPH=A*4FxKRM zuwB1aIeWN<_1Ji{GSFbs>ammyOoXqp)yA&;RHV_>H7t&j_j2io^Vvrw_1>y_*P{!X zG?*G(mfsEJ#-n4$Nu`*Lm-A23L>*Eep-`9O;?cAnvZg$#{#C^1o$pHRtaQb>rsIii z<$nDoQo%A{aKw(9Ry~i+_NI^jqmy3&fEM1%IB zE+oX$v#cY(c1CfnL(jFHH%k(af&oNVM_iyoYpz#+gO~do+;8%E_)r=A1#vh*N_K*j z4?AeJZ`ih9joC12^eF)Fh16>@Q95QCO!*6(Pt+X4%u;M8tFEr|HdO#2tSmv6o|zbM zw55OiyM=*eD!e@f`E}Iu2OaKow9ox`Xs2gzA>H>}X@>Pab@j0_Eup8AM)S z`RLg5cx@(02FH<=ogf_0@aPgPvua4T@Zz+tU2lFu>)v!r9QF>f;IE9AKpCU;pw8tY zto{Zehr7OQ7wcBUM4{@gY;@5^Zc3G_lAD6c!75B57BE<0tBqXl$dOqXf%=i11v3Cs zjw6p|cVYs+46f(4h}%&?p5$vXY;Ord%G`T0fn`dfR>EiNg)=257|_&wpeFBW=ewfm5-OUe1XG-_T{K_+@(LinUT+S+F+A&f~COn_{c!KR5*Fi`l6XW=n` z!KPntl&w3ZInSj_~xmjhQJz4R{E%ql%_yI^rn$HnZugY2Q9G@{i@tuIN({nYGpD5kOJy zZI`;V?`rokx+NKVbh~*5)ETwU6IBR_hMk()L`tW0w&+qnKbM4w^Tn%ona|&4cg^ro z^RD;sj-p}|Rl8U#)i72A_RsgvGCVOq@yMxkoY@{ackz-NGDi36?&c<6;Z}8S;K{DH z`;#$ftC&PzpcS}=;0vqyXQ`bK7*qZ3tg-zKfXfVj3SpUQP4~*`zAeI}8a(teCQ^S% zYz#Om?H`(6R8dv6&Dbd<*hPGsRmZ|Az4TA=yGQUMT*<3z*k3tG(l~e=>dt(hDWFUF zdY7&7UU&V5*1b^XYg@|=w#r^yzcj`Lh|kyu3d%9BY-Yi90589v`Ni&d;1S6z?f@3S6+Wd)yp*z;(zV z&<^wYx;JEx+#Z;VL73t0)=n21I)JPB7y}~5HbNOMD4UxDo?Th*a{b)h8^{GcOzQ1c zc*Bn}E4#7U3gYlzNOj}lyMK=;wWrJ08zDo(LY9(4uc!@7N$k!H32o#09r1q`^XzYr zI=s-!0_gZ)B(R5A_y=R4L$uWre4b^h912|&cR>TwQUQD0GC1HUY_HF#p>|Wr$BLY< zd>(Gzo7pq?;^EENAD5{zrSPLiT-PdH>P%n-6t1GXyjN%C7>HX8o?LYUfOd0B!QU77qi^M>plvf?bI%g+pW~>U2x$ilN+KmnYjvvE5`KWjQ zdn8pVRjLq|arxQZPSb$C9@h-YhlDpfw$B1+nhov0eXzvG%P0BxX5l^J?b>CLuuJcn zBpsAOAEk)Ja)vOdzpzy=_fGF`H<3_T6Z8@P05}fdn(lyW2EE2w&!}NeK5Z^&Hwra9 z5C$6p3jXvgbU7^ImK?@!)KU0{8Ezfg?Q~aI7Mxd>ESpQ-^4pw~WCt}Sc@y+O$laMh zS{|RM7A~HmL+?3>*XKTFwwLwLCA`VZ81u4p7BJ19n&H6LIVG@U9M`mBJKKEkhYN=< zX7r6eLxYkmUL|M%cDUU?RP*$0>t49A**gH9kY2j}ppvmmMc~|)gi}E1(0sLhGvfE; zw=gqfJy=$ltxT~>hrg1!e*HN~Zh|L?W?~r%0*B_#iLn`l?TmZ@P_fPu`VGSfv?tOlFY!kmVCE; zp2gLdVrOx+z!SN-+D^#{q7EJ7b<1o(gL?59rH*4hKj6|>q%l@zffJD1Kq**xzR7C0 zKv9h}rgr79YN<5i$WUt2;}21nHwT+pWW13;`@*-{ahH+Rsa=#3eJ?*eJnH9*&#ZXi zSJ4}9hHH@7tLZ+~zsY|yse)aq4+w&!u|TVk{&Y@`WoqRdr2$Yz0h8TOi-2}>%t>cW z*F27??Hw#~ACEdTcwja!Pw*5iV3UsemS9rReRxpw&HZ@}x@l&g>PHW}YP9MW`NA$; z?C3j+J>$xEg>wtEbBW-vPJ-yum=&3K$!$TPg5L2fB%3tzIVw zLcgf?YinUU-*{N2%sV92FPc^X6(zN+9%=k%$Gq0mR^>g@@bm*2ryb&m=$AWhO!tNr zULCrnl1EVdUDbIoN6vnVic0hLDs#f*n zn0-`g#bSnGhV&-k?V7n7HFfp04kx83VHr+=k~w}?LZ z<;EWT@_B=FqxYco_s({i#|?-3bI4!r{SxuyTHi%1^LhQIVRbSZF}Zd zqLHb*YmwB*v&xqeUk-rl+W26S_3CNR0(TWMYBNvKBl z6O!Aa|4(H~E`@x_o#Kxy@cIDryzPov=DTv((Skb9`NjycnDrWunZATN_;==^#=vSz z!q%13IWS#4p#$C(+nD~wGQ~`KQBOfFf;gP|d-C=|iH(1qZ`A4D?_q%9OE`_mVVZFl zujPGRSB*g$+m`mNu#T6k_*NehcJG2dfV*TQ7`ofGc56F|6avN%-uPI$ge!(qFV>AZ z<+Ki@Z#N?kBP=PeNNo(zZhsi0x*^FvGU3jQ$@=>@;)$Cp7JX|tPytjuz8bgF2E=;v-Lex5>`epYym|pw!7?>M)A@E?l zf*8V@Qt|NKq#`YMJ{qmGrhe6D2yw79|5K|~;k8NglUR?Z1HXhjMk`;RWHcU9#uRD{ zLjpQAM0n#YuFJgRxqdzv@{Ws*jEs+Mp0B0qMkHHi3DVl|re6dJ%7w*)8ZzPtOoR z0#2MM`W#KD)cMt)Ng=Q5!}V_lK9p#ZI!+rKtn{|__S)EG5DuL7lfJWlok)_C9k{X* zfo()~kvB3lG&_6=YMXZ_znqUxNTL{^aaR_W@|ee)oudqPZ4Nl&rpT1=J-r;fbSe7d5pfYa*+JX_6wky^||B#CI;IL1xIAu>MT z+U~DyxGv1Cj|g>6Xe?eK_Pqa}IbYUA4KPbVu&^SV-fYqal_=_bj;x)T;T{G{>4UR= zOk16nf*8S;-=jY^yr<{#4g0RvNP&xHx1&6VRZ_xp;f)M_{eDI)xgCV)VY?JgGXL`H ztyF=ua+|)Z8WgGLXtqvG#?#8Ko$hV3)+7h^ei)?zFI|1%d@lAZ!RyYQEH{u?&kJhp zZNGp6Wb({3SE3ErTR#1y*f>#K``j_2wI*T0W%A&6>!o73au&nie8irARPxG7M_oxN zEghDlaMY0|xlKgNqhRMCD6gUB)F^e2Pr z$Q~`F+%CC`wu-p#MdWQ{gZPZhO-joPv>ddJbC-?jrs5Pc9xkw|I}4n`ki*=dqO!z3 z0JeK6**LH=>~3fAIrQUDi*k^|OIR*nDWpJZ7s=A!i)UZ_-1*L^09HBn;)H6lsI^JK zv32ZXx-0MQCHmtgvZ9#c`v^@$WQiida`rFo3DD5E+wB{dPTqf%r6uEpyFgTT-A;nm zl>b-BwQ)n6Id*v9je(apno)PBI^wKf|fVYoZZZIVJ}ao-tAw+u2}DQt(^3)XIwdb7V|>;&y=nCtip3)5(7?;h{^dWO#SUa zJ+j*|y@0W^FIHaiTnkkzz!$zXG0h$yC_m$_gqMuIcj830)mxBv>|WMhGO;!pQ_6Bk z4!n2pxf}oft@zzmwYen{(8%r@lmRN z{NrF{g-(H~$BWy4WMw4C8sD9rrK78pX-FI1vYsKLoC(YuihOXxC;GzA!tG?)TJsWX zYr7Dx3H9~S)U{i^fqYS1xQppUdPPor+n2{qmh9X1ttiDZMxz>jH+U>W5-cSpa;c68 zOgpOhij=PA<6BM-qY!WgTH zM_<*2^F>Cy4%jKy8U?Ug0S$ZkYFN1B^LCZcJ&TLAlFgIiu16pj;>QD7>{t{PlC&817~kDLU+~wodtm3Ch;ZVz)M|L zYz+N0gddVmqXpONZUMOL*M~NmZ}q5|8Rp{C6VCEeP~3bY#c&@L>iAF*odm<|azAR? z9WV8)=KPt)w7h%^xoGfowDB5w&ZX-dinsjQoZbyDmbIa6-iUh9vs~FC%j(W96L%Ks z$jwkrD_l+QEHW~fQu=*9RaKTKR+VI&rHO2tQaRuA#)$psMLbYI0Ikol>lO7X>~`G* zdS&+@f@6l!ucD$ZT*_i)w+ns#oDuSj_sSI~52v8E5wrQ|eA@2~PDFuCJIO|xoR_S| zX4}i$Jm~ZJ7oYO+@yA-2lFT(-xR_Fu|DKFgko7|o+W#wWJCF~}i_P!6!vJT~RloRs zjs~qz$56l|Y@`=<&$GWa8z?m$t)CU=>GNrOJZXh@e?1JX#NVVBxgzcJRfpe=Bkgrs z8)YEH$u$;=fD6JV!UHKa3_SW;3c7hux;I_bT+2jl_^WVFLqfvOhWcCt*QX$@@KA8( znirTG@cw@iZFXJdDxp?2_mPa8AlqKFBnPyT-lT1JRju4^+Wpr=xbTCWNH!d0n<0 z#opHDoE#ut^!VOWp_5<;Nx~=p6ejy@J93rz0JkskoU1h?A#M5sb% z{l#fqDJ}$KDLi|1cVh!(>0>D@!_*3b%iEaXeGOEt_f8b5dmkI+8ZnMLO5=8~Q2}|b ze6aj(oX5F5eQl0r>1Gq;XmL%rqA}$}SArr0VykWo7=!Jr02xsUkyF*vlxr6X@qDSZ zF6Zhy);^VN*1tAJkwRWyg<5e`eB2A#cp&M{+mO~cJni$x8N%^)*e;nn>Rw)6)IpJ+ z32+V$ju*l6VK0;4WwM=<&+M|-naeK5v{c8)k}G3(R_ntZ8=lEdOnkbdoMutWrGsR5 zV`#5HE}9w4P9s>()f|vVH(S@gAX3E#qlXz5&rhKtl0x&lqKX?_` zcFYG?a?{hVUy;xQa(S@xuC$FeLZW*|DYHL{lw>h~W$*6Zht`jzF;w16-zB*O=gjYG zcwBf?0e4=&aeZByZifj~-(;w@t{n(O$U#1|4CMs0Vvy8lM$Q0kQZu)yZ7K1pHltG+ zVTj1?FVydyi0KJr6A`|2I5tMAt^qI|HqSzWfobs9|&g5GyoG>u0ej5Ua2C zlVA8l4h17$z!+tyD&WRbF@Z3n(c!fXj=h`ih^a<$S7=@x6sue$cSzQf{`S z7P=P9qR^tx+%Z7E z_)Ndl!e~EWRAP?>lxoWm_a^^5f+({pm!8y<CVK zf)wJB5tXw{H!hP;Gti6V=bSU7r@JmCW|Y@aPKwvtKYLaZ`-f&oRF`vD&8$3okH4VV z-aTsgAzG3JU`-b`7@uEf0n$V7Z#&Kr)%<1&6Qi3Y;|-Jvu$;|?h~MNl655BuhhO_M z?A|MhcReBkla%Nc0%|HNQMs>xF+%nho16W@mu3J7{Jb_NJ$wXbc(-ZG9*Fag-9MN= zk9jN4S4rNvU2sJ~T9JD4${9$0yfOQA9wH!Jz5Et&&*h#7-p)NGq!OZIWFA`4-|=WG z=%;-ZH~1uXdjdTDT$g3LOyt>fi73}Y>`mD|#k=A8%Ex+FLcK43zEx@k35pmZ|1$Rl zetfd6s;%i*v4p;iei&>=!ubfAf`NjeXrA<7{v2StE#CHT_f8V{CAXWVaOb-&(zsjG zbJGLWJp_3BL(F`4#c~X{?io6jGj8#zAYNam4?7xd4yt?6rv|9EFC{s_#5~_iQMI}X zFgc~sXZ4E1C&zl`hOa84?x3yF6LZ9JpNEF7-jluehDC(A=SAMl=`9uN!|K_FJi|le|nvH{tZtib*z*QApkLzGMH`8)3I|>5Tg% zOcRuarW7Jtr^$6=C$S~A(wEQAfX$xC>lj_>P7o?cIMO#UIJZW4XG zM-;)(*-NBITWWT6Odx|IuPYFs60fk-QB2i7JxiL28Yc2npSb;|eQ{-HY5D~INvBPFR|Nbi|Y%>PGJz$7eZ$m=z>ReR{U&cudyPtgi$_wCy*mL})V^cv% zFEfFN7Zes}$M*tv`z}`97LP+aUCx?K4|hC=M6%VF4Ew}yzFZNBy%eM25PZTAThMC} zrygqvBI-`WJT}C(*13ncg)Nby)+x?uV6U?2UE`6#(;oyMK^A*F%c!~Gl!y6!qOW|n z$I8~pu6wfAD8L#GqNuAI?#G9V2zlr6?x9o*eLy8R;$VFNK>Mk!1IjBrM@eN``eywb zev{(FfQ{9Sm;938rWYdb$XMTd39tXud@f;I`cckSOG)Kxcr;o0?krczFu94sp_!EAP-?q>}WzcMZevI33Akbj|fW)|)Ren|lvW9($VIP{x!I3EIN_LRm z@+7J`hX#Jh;_*3H8Eb(x)+AO3K41Y>5f3X#4DkC0!JG!a;{C~pa-+b!%)-8gkQLeZ z%$zdf2tQQA3$%OR$zZ{~YX9tn7iZ1YMAs~P9l3mh;+EP)^T!~A5yQNnt||PCy!P9( z8J#A~A=KnDBZ-sK4(ucgx!CMi;Q3*TZk%oWij^_w0~i68YFjD5zF!Zw!juy(k)!8?nZb9ChtR?EVZ5 z5Y=N-NOVgaZ2d!lY;2CJOqu{dpt7Q{2OP!VlU|)qB9M9{Mu%W{NIXy`@?yo^_#7&IClL;UbU#N-N;&a;Y&#ryJAcpW47(`ugMZ+js(GT!okTAD@z zm?yEvJeIUq$C>uy2nV8*_<+Gn&W@AT&7Xv?v2&FbmLfyFpMjM8iJiplWCtNuCIDoh zAMvr7MMYSgFwSsRx1f`x%}Mg#BuTW~qD=U-t&Bn(3r?D;1|W*+z*;n)2iC@Lvl#1+ zzO7krrxeqTY@fcgV6$GVQ`gO9EG#VaD=4`$u?Gf|*pm6}(YL5CoG1jw1RLPq^qQJJ znvXZScFF|cPq6As3VR_ueS}g}R6IOXO2t&fbn>fIIbSiiXkmmoE|@y{%{v=SY7MA2 zHDXwJIGLAcAMe1x$)1Pj`Iqgt&U-ASg@t1u#{T?4rzw{@CWV=E0+K2$Y}LD-&&0II<^T#QC5@-=Y@a>H4;g2@ZboQE zbb{8n$$3NAQf;6G**lYH_v`Dz{flWTV8@6zIq+#cC6+;*<>7f`@hSe3C=+2v-}EE( zadq1O#;1?OZ>?u4zNl)bXV&Lza=%o`R|}R77k+P!C9rV&_I-8^`Yu^Pg3b2&-b4mB zJ8;eu)M|)r<<3jW>l3yc&zptAeaU;~jqoAff`#U5>>00FRLI5AI;#N_XLq0i_hS1L zredM>ehy)X_{LZZE@4wMI?Eo*`G?A?x~DqL3%-ADqw|XMVZ1L1L-EK-!|2uo5u90_ zi;YYpQ@V2ZJsv@bx!j2FelMrGO$@#N+gXttXGUpr;fb_Ev3vUK`YhCD#e7sv>N&ng z!|e>J=rW(@;Rq2Ao}EZxjoH9UOYKSwNZHA5HnHQvD@g0#q2{oiFiV%yqxJ$Djis5P zmab-s>oiyvIN0MwvA|}$E$@(shV=YJ(uZe66{Q~ka=(1bb80*KRy|7J z2PKjP0&%%;SrkmV8{4n|*kQEC)|v2GhPBQ34|bd>6~(beOvy5AT~GWZVFPtS>UW}Ve5xnO_5Ez=lTpOe29r#Ai6z)&>GJAQ zjCPN2AgY;DP`7@tjji4n%O7V0XZS47H~347valvt^pPM~=)}WWQ8k zvzwIiNqbbyb(A{4AOq^J7#@i%4i0=W1Bpef_Z!>@_x$JjfRPH|jkW%4Vrb$k;z&)t zOJ06@TC50Vuzhp*D&i$&R~Ix?ahyKMW1#&=#8R++fD4}MXS0IjMjkyMp*~7w zT(#rZjbvTid%yRJhj(NK3Eq3y(CB%uIySz(L;rIjp2e#cqvx9>} zAvs6v<;<0G=W9Jxb=8cVdeo$r>9ED6Hr^rDKa8yntSzB^M=}tf&<0FO?=!oTtp!Z> z1T6XqW!sHXQ6ATe=OA%y?zbn*H24NZm|xMEm<%R`VBpFNtu{-MPifqD??JUx@YvY_OR|=K zjZwUuVOVtUzz=Di9w%+OKJSk0WGpMM7T&*WLV)yYd@o-TBzwa@$Nf*>y^AekvTibU ztc)GU{>$aT9)s;j9VP7#-#}yK=4gNw9UbTMmwk{z1|AwG|HEeaNej5*JId@H2$`6o zO|%Mq<;^G=6nW_Y!8G^wk9x>CaeNM{W={vz8sp`Jlvm&O+w=PGmjV|)c>0}B~3kD@fkgrbm91L z#Pr5kt@9lhU7q8ygI`i^(zv5r{i(lxl)m2(Ju?dmq~GleA*JV$Qg9~?s1<)ajr}}$ zx!bVthI&^tlHcF;{%7uay|Ad?!6*l-aK7U5bhgEx_kW=zPXNmWCA_HN#KXhbdtfQ% zmzH$5UMh$MEaZ}=D8en2`J^1ADW}P-!&y|$zKBO(tbYd}jDYCV#FI{V*R0T%O`<4{I9xU8a z4`NTf4ZoJ#bsNoE8;T0dBueP~w66Qs5q+4e1y;?ksp%qtdfq20Pu^8tkQ7Iq5w<)N z#3JRDI_BY!bAKn^2YWgog~&+8$mHRet}f-C8${_Y^~7y$eQl+Ggd@?Yjn;Y}+C(rp zzxn0Hct9%Ps~;L-Ug}h0Qc9ttUP=WH+xm9u{#*|k&fvf`#{_;8+DUg2ovE4i8<1xV z);|IwY@&~|tu!Pb1GvJ}A}`2E8ysnoYj#@FT6|7&ApVdzgGNkrTEVw(Q52ciq53W5 zH$Sf&_pe?Mma7!HFTuSgpQ;R$AO= zcOtj5OBWk1C$_ukZ&~SCq{ocxoyKaqqlT#;ho0`seNU7scRe?GI?p7gCQi_Ad`{W> z4?2bszr1DhT|-58Y^@0&4-Wxe97=ucrBg7UTSbEWU&6oA-Z0mTq+FhhO$+0 zDE;DMxF@)G)bm(YkFKYFW!skJmR#@mKEdSQst!TG#_K!lU({b4E@cHoG5+Im-X)KC zNzpappBXF{8X#l!6OVLe(6VEy(Dd-1y}se6`)JhPCX)Z4f0CFY1|t#yFst&Lwumn= zYubT$^DuAON zKa|74EyR%+xN^hEM4nq`lYR!7L35SyHKSw2>->?SUCUSAL4#M38X9V}v@ehjiB|0B z&%Wn)l_o`iD!CdJUjO5(ZUJ~)-Mq9EXyYH87h_*$+j=i|Eoj+PN3f7`iSvzj?lnCx zE$Ptx(Yv@XUEh%=3GTW$sMI$lBnbnMQe2Lsxs{Z)e4oV2FMeKD5_>5@!z@@#p)Y-# zJVSENjf6?Z+_@duSd!p9Gskmg+qiBA+-P8eA=W8M@27s=(yihtg+zHRCzhCW2#anXjBI0 z9X}EFPw+D_EbIArv+(s^)ZCz8@U1bYv1Yk5%F9dn_T{p!*Kr%EI&O4i3D`L}3E%QX za}XiynCy57#37j>l6LPldA>1xo4%>eOYa7CBmCo2%<;@r+4JkHfRGUB6*|tejQ7r1 z5-M{|d=m|k+`zyU0V?}xU+ZkyBi>^sK)~_3&P%`wJRsQU41h!u@)O`WSKcJT#H2XA zn-bzv++>xxo-F7{2}htm3hA+bjdfI`LCrajz8pIk@nF!eH@@YASAl3cvVm;rNkoSM2TI>F;9PBO3cU7MA|0R>-iiWP9v8)30JCZeg zq~|M%Y;+%S`>IhZxTuP$&h@uhH{~SXbVt`5*haeTsn}&nt}ajgG;CO1u3Xx4OL0L)>5OM=`2=s*?>>+M7%xvx z%u*6TZw8X`7|)R(2p`}jCDtkFHw7F|U*B(tDfaj_&zHNHSq;1V7#9s@Dk3vO#SM|& zP?jYQ!^dl8pSh-xm)>9S;&f8B5)FAFm?ohHe)}F68%}U!e~bXx)7wKg`!~B46-_6lB6BL&Ti^WZ#Xkk;GqD*XK5BR94w_ zWyFOVMdSWeeu3ESvncjUd;dzf7Y7>b__B&s#z~44K>e(DvVa%^NP(36A2L~~e7P#; zlx!kHQy}pv`^};*ZTGZDXr{hy>aqDT*ZO@Seoyvj%mdrMVuRx5cy0Jwemjva+;TltYzXOlmGn%;tv1Ku4ugZI1s4p%XWRZ9iorT%cl+~w)GW3S`SyAj1ox_@Swd1Y#s1ypLS<- zx8Qy0>36Toq0C-px=lH-(#!F3n}2`&0@<~g-1|;hFlc;WOwROfH-*pg+_F%Aq7#bz zRaE=jWG7|9Y+zs%VBxc~4{JKyyknC2iiRBp*xJ?8)6?p@Nx=4~LA3bu@k~r?vQU*_ zU?unoouc}nPgcVyUuv8YZeB{)^OrPq`)A2*|3`0xRhUzTp5K783>p{dC4s`C&IF63 zKCC952;5}cO+B?;)*V3WqXdK@4#hhemQ2%i=#WH$y_)f2i|l{7OKrGRogf8bJM9jN z7(Q8Im%x{Z<$fFGB>(r0fRH8nLJM1mL!DR69u$zM=6&>b z&;`xBA4tyH>*|_Du`nrab9Y|=U9+&k!%8#La?5S@qM!AM5rh$L6R#Lp+mm7TippA} z^SfM9O!ytJc>?t{b>GwB=zze0<&aB-#dsU9uiC{DHvuk83AM=Vm?Y%M6!5&!cQms? z`_A2US)b~rux)OHP@RzY@oNasOeapKaN{?KKPPPSjuITCY@tZ2b=m=EgU%fS#b|_e zbt%4Kh93UwV?}^O^4}7h@V_n8vOqyb`?y*c4KL9kA6HDt4%b<|+hJn_fE44du=$Mm z5zQ1WEa@E%$UBITy68DC?m=3>XotfoT9V}ExvCNbgs8qxpz{Fb&)N+c_OF829*ewV zxj)Cf5ERsCEs>G}?Tm|qblgMnRU$x6YOK4+2tSIuDMv2MDfJDl4jnJQ8TIJDXd$Wl zZl67HjUU=sR$1kj8B3d7lwy5fT5WVYp)PM*kzYEaGc8IJSLKSU+Z#5;vADGR^}gi3 z)P&p0So{T;^n+FfV%nbff!90sbG!lh$w2=wtP$jD!*XITVK28KOmv)K_S`c7eL3g~!z^`v+JyzG(EFxs(vVI;cBEP#@ z&3w(y)qa**1A_nJPI;w^^_2Zwc)@I+)L=rz1LuKs||*HusX{ z=4;TryuA@=F#JmP`lKxVjaTB@=Ef<=6s(|^8_vSw2zpazkJJb1D8|S0w+;^ag6wX) z9X(~Y98E8_EO(1AbSaAW9~qG}8{YglOdLVtpL3f(o?RUN*w}ET3-R%b5%pWg29dk1 z!_N-X(a$@dr)A|K1JV#@5XW|F3HiS54wS-X%HBqFkpa$BHjq>9bN2XzZGM{*{}J2c z0;*C2Lqo&_59zG9JDq)kYs{bp21f!3sFP1h78<=gi5Lj1Qj# zEz%rTdVdF$NQe-Bx1i-pNr8?T@1}6yN4u_rD6uFm#@CFRoSp>WfuhTJ5r8oo;_b`5 zS5;HWzMP)!)^3#t!MJioK^;gt6tB7|FT#kah3LfQ6!s;lp!xsAyV8y%j`x4c>#DE@ z>0MZMo7&bmjK5Xa)C$(9Y4I)k&O|I7Mm&mtt02+0OY&QttFattR;tOp>w!u7MQf;0 z^RdXoJFjAaOHaumj8qD{8QS5AzBC4;892RDeI&pK&EFKg*cxOXW4HUbj&M_ciF@PR z;mzr|Af%7b0Pkx(056jFZmK`VJ>Emg9KQG(?XL~R0CD4Csvz0pkT;QXmc<71D%??- zd%JoDR!YW7o|-!SWpVYTDw6U`+xH&f`rNnQMwgW#KGnsXA|01&9PoR5@Ry&e{6@HS zNpXM5-n|So`m-Hsu(^pkbJxUnHRk+r?IWFo4itftbbQYRC7~+;(Lv&Q<959D0)$d4=>L}iQsR_(ZOumKN5k5vw-vR`-D0vQgg^t?ZRgWtXYNI~dq8H+r@ zZv0`K_+|V-Rm?w7lf7uM1DMtZ-vbfn6PuC>ww2+@!bBfJ&QvLg^z?a^U zdkTr6J|Tas=(TQSAuH=rf5|}M@Hl0>rdn*&;c^giV~cT9xTbv#0zhpnTIGfNx&6|1|1>T z+Kxc!8hPgJug0OCo~k1Fwf@QY9CfiKQ#atPkQ1`sFAnB4O-)XJgnDNBTl<0jJN<*| zfj=dMK(I(J{6`oT45NLz0gkLWRQRNEo_OO5BIOuItK;Gy(_@4_s^p%}r2%h-y28a}zm>40U z+1S7v;DJx|<;LBT?G|74aa5pe1@e*sp$8lc4aU_Q;mBwIYTqaFB1&WI2$`fve=^#9 zu@;JD26{|+S`V$p25CWe_1rZQe8i34vOIVge`hyKevg=Qey`of%OTnNvD3eMaObx# z`V!V;FkYM6BJLsI2+^%>37*5#Y%y<=m-I0??;z{^Ua$Jj(Wwo~jXGK(($vO2PYzRv~cQQytn4RaKpB z%vXCN`}_bMhcJ(prkAZOS1!Kx;@JnMp>F(c7BB8=eXOJzF}+Q|rTU^dTk(+m5E^_?NzIBLhuY&=`7pB*-=DCEz=9I_LX*uMEItJVU>fzs>MT7Gp4W=h z>K|8EU-R(rq0cD?v2VnplpXJe@&+I}AziiB*T#cy(-Cfv2ImF}W^?SNWz--to|Uw2 zB@|M}N|CuZ_58P(({MEl`%h5K6bWcnqB6ujnmOBP%PKcIoW^8DpChbB0v+H8nQY~y zP2;~&G4W`S$)jzqqJ;{|IQ2M0=kanEf!Fdtyo%|{bioFMfAB*O#aE|E6AvL2M~6w# zRIUeXTRlUpNJtew-YeMwpNx!(yVO1gC=A*@*g|#H?}pA!wEnWxdF9j76050@qkhZc zda37uqG_yGL$1i`0&)wDZKu5BZg1%Zm!@sJGiY?KRoH0Ckt2k?z-l{`OvwB{rCII% z3Ax=eGT~qn8lx=gbKP&+;U{^oQYrx6*n)sL1f0~&5a>8`d2!J1C!1+E|1WW|VrpOX z(g<$?OmCfF5L$|jyu0oLq6KJynyXA`jx&Oq8P8Eo?e`_m$%(S+KY|VZUsM!98UBpK zwRUPTBen@&wJt~*F^%?p#8tX@5C4JBRW47C`kY+KAn_~1~)3@E6)@p^mv z@Y0@S>~*zwBHAdpk#*zmMoXIKl`|L;0KqQu*3SGMj@b)*jJW2eAK;!b-{F|(euC@VVuZd;>_%|_(a7&_Yr9Lyd217O0wUWHHG;}lJVzmjB-BJZJxIp zXMA7h`+YnA!w_UUd!K(aeZqnP1Z^>B&Ph1GEjE^abVvwf4&+%9HFKT#Z$D7DjbxpYqg`^lMT8V{T3%nhG2;&sT99w~mEXi%lyouT zQZiA0N{)2A7+9_CXe|5?t1L!8W6Aw`=bZIV7)9Ba^0QXg0FfskzLjq3_SHB|0qnT!O<-Q*HkJ zi-_Wm7kVBIZX@#0XpxPrQKgi7^|b48ND^U_?@=KZ}|AG(x`d?ypsmKP3wgLQtx2a3YiG&FqzPN+TcEH)>NXwFuF{ne5&<3Hjxo}IscQ(!~b z{-+f+)CJ~*rxEsLKrQktYGdRL}gsHd&+Z3xFZ&^fn}QPC8>^hTAo zn+PGQ-*|t2*tes#=Rjt@bdM3$hD2#=w;G9P>Qv`xshFF!oz`ywJQAgC*YrRs1y6@H z1L}y}T}VvKnPkG5Ts|kc^KiSTdJD*-S6Z`?>bzTTQTIcDu~&0fgZguW=MyDwL{SIb zGuBd{IUfPyN;j0O@wjk-h5A~ez{W!w{N_8>f}=+tH}?>?1?I~(cd?VeKez`0`6*?o zGS4!`9}YTMiKuA=^@DLP^%G?VaD|{%aZMGLaqp%NIG&Ld@R-v?`U3y?+VhOO0;$3~ z*kS48RYGJsSvtSd4DKx5=BMcf@Mk^+KNMAWDEW!vZOHVpaFzqt#qJk=nyu!mp#+R=y+bHenAlV0A_9~AB=8-eu&xj};1 zXqadzSM^5Qip-_fF&3}0US!1O#r%zKw8A~>PmW~nplluNt&Dc(t?wP;F0b+ZyJ4__ zN7@gv?B0K1OD09+Ykw$_;}-B7X!XUrci;%oS!9HoD~e(-0XV~@8=g>3qDOq}E8mn> z0TE2TO^0o7W18t-oy)=vSC})PD_H*=$w@vjJnvj1lj#KoL{UdOFf(*v@mz?;-2Nfm ziqXtX$zGX5e}C%f7SYlvRO6_w2|V^efS(a7Yw(ls@y6whO^88y_cCA`+Uo5M4V1VF z>$J6*TXc||#re?U+SIj8IqRw+WuZznibKXx)MbIMG@<@DCb)&t4L^j;Tf|B_f6%U4 zrnpvK3`+X&n}Nv4Kjtncr$_OBSzC)#i&}{-Od<_|BQyF?YHSR2pJ=fRwPWlPg1n?b z(~YC}VS6$3P6>(E)s@1NiRaW@Ys|QLR?MW8=sDdv+MKc40Yi}u4_rV&#{!0mYa|hX@`iA%lZXN}w2n_~bP1!Nza?pH81K zEw8T7=kOhc<>Takay$Hzzm>ToxQRW`s2m?98~sAZjK?Wa90;INTL-^r(8vDueE^k7 z%jjvgaOzNegR#1&MwAYBb5m)UIi5t|;6x=PV9eoHD@&^r%GiIYf(LOh%o!BjN-KAF zoljgmrr%^u5VqyxI-fpq6dOazT>%G5r0Ata3T$|rFlgPw{ZoGHW?H-<9*r;oj(PsF zy23Z00h58<=02H`i_3R?)oz2^9o6#cAY6vVoC>}hi;c5$0oGrqg8`+~geyT=@01@M)Anlp_Gm}8HttF%%ZeSd{VlAU|#mN%BP zAj^#^>1d%{K{e1j><&#}@*E+I zBh0{Uvj{P-aERg9slH9#Tb zzEKODmJrMOPxGE9;&C~4;p&AGsJj~nrwyK%YBhL;LPj^C97PoPeP2}SHN?blf%^$ zN|gjzS!oqQ?sUGLf6({*P8KEuz)ij|FR(a(_2>h2GpEcc8gqKy-xYTgRkxQP*&@CBP1 z9sC8$w-aDiI#&g!l`lsBy^^Va1o`vU3DwYu7EYo9hJsP5;n#S^It-Ks664r)Zgn+Q zqGHe`t(7h6u6gvY#P$*};jyb_ieuR3j26h81FXZrQK^SeNF$$IcJ*<9 z6i9_E7aEA3bhIB_Eq4Kv8Tes$*ErbEYC%LaS@#mLy+}jvx>nueoof z-uI+&E{pO*) zDU8jdXZV_*5^Uv6wZC^ganth*z3(MspKlE>#=;4gyLVVjeXfca3lo7ZxW*j zAy2=c;Oz0f`Y4%_3LQn{m-OF&#?rguWNAz?9`laFv@@M}&4RaNeJZ2_nG!wq3paZa#kM#KnbkH|q~)^Aef%kBp%|;>=-Ta&k+gt&!YM z98F7%0^9oA+`mFlF`!m(aXt8 zj@Am&nvIo$OaV+2PpQEW-guYG0hGc_ns5~H@fw!cT!IwM*Yl1ij$7Zp$sp9HmzcEs zJ$d{T9~p8!uf>R%hI%Epg=R_w$4)z{GB1`VZhbtxb$serTO?!e`qMKCYG)&Rx}xVEMqar_YY zVGwrmhamkN6cw$y9EiJ3O$JMUo&^SPB}~s4928=LKwe*_ck5;VI#3YEqu+@5v>03| zQ~u0EHJ?*z_eTZb0}yQ7ZK$W8XAm+p{R+fPE%KAe7tA{KvPA^7Bv~CGygQaQkU-q} z{)Ye9tSJ!tN!k;@zg+3ofF1+<5aD>vxONW^h;aRyeWeKMR%J7Hnv=jPLt0wUmm?C5 zZ+R{R0#)Ecpd!syW{4)CC{|eq`#^)A_@1F-C%w{fYn@||mRI!LooM8~(s^M?Db|&& zXFuUR`zCd|zt9Fn^srb#>$eOf(~9vP z7p&EnW1nmIA&URlz$lkw_35<%2KrYAy~!gZGGP3bA}yE`%at|&ffcJyd~d0~VRrSd zw!-ykRcnw%tERLM7imxVmKU8iZX}Vv&^hsRp(20N9I{A+1%}n+R=0#%;chu^Jx@w$ zk$^zf-tOScjqVc4HgnsTuGs)KAJ3-}51%lnrgkB#P2aPmknp|o%#6(s{vVRt5Om5L ze{2X(aRq8M_S6{|fFo4Ty;s8JScFUBc7J`YrmFo3{{4T!=-j#pkg$)6n2-xOy3w)N zYVQ^WlgcU8dtXc}PDDtWrT5ky-PEV!-io8jH&%0RM8m<1lR?lHfwbC8U(Y$m6{&|3 zehUs|T(N{e`pu#zMVgY%YgSdp<&=b_zn+3|D?X6+G(yNhk`t@0c`vEcrePX}PhTG( zTiP>882a8>*FW-Y9niIoqwRE7_}jSrPEKd2K)cm{WyZ;DH`K=bScj^Rhp)$h0)qY- ztjk6CfC*nF*4NC}d^$ngRu!vq_0HVGs65Wb{E)~}QG&u%v0u&Pc1g{3mOo6e>+~em zOW1TV)IJzmX0TYf=D$d-u}(btc8v7~5xf?-ka^!L17%{aWS2?-*3B&dou#h*v+^au z8Wn;7kOrh>6I!zyqTd`58lvR4`bovn7WJ%1le%Ru;-QU;X)k%BHhRjXZcm!Eg9J7zq<~UPIqC51u3$)J)P;N4wb1TU9 z@E2g;d+F;94K2tLny$a{gz@?fK%SKWV_M|yK*Q;`JX%1||L{J{y5&&fZ!7XwCG)RRby5hsc_yK@wM0=PPfs}j+olv5+5%xOCx}&;zW$4B0~Pr zmV>d`&fvXE*Ep1_;GGpu3GfW+@x4T)y_#`~buD_6UgwFwaQ>U8RBtl;Y?pOVx!9y= ztMoH0kyCkV;;%;q|JZu|099#hxc+!LqVCuxsps!g+26B*3nsdX%4zf#xf|Uq+=+~+ zss?(sI)C@wMO^dEj45qAV8-7p+EcV3whTpJEu68pVMbCq7&~AT@^A0zAkFv1+6A!2 z-{}%Umm%N6RGY4_%x=?r7ymLNO|=^(7?)Hb_NfePB-Uq~62y621#a>xIb?(}LXr#p z^qKj7&xPYA$rW3Z=F8*G{kvC&D2BWS-uuIBsKPj|hz%DH>3~lhT996EY{^>g-bapP z_1IFvi8Q|~>UC;SS(c*8xI7yaJ#X5O3|9FmeMD*NvOQDg_wgD#)fNWif<*rX0)((+ z*>G3Cy6=1CE`m?xh~4ImB+w(efA79Y7&B)O0Bm*USm0T zpNe+DWcQ9FCZq+85ri7ThQHHiX*%3fmRv|y5LGv>B$mVj|BUbWFQg;>pgX#s8FXYn zX2Q?FhqlOh%L>@9yf-E?A^VK1>N#?{^%{)(scZ!Um3(j`|7LqGc-!<&r4qLO4lanv z^NP`nFHBQ37#aB@cBm+QF%*nL`^I#PJXNh)JX<`m(WJt9*Q>D21cVepBQRI1RdTD| zkzw%fac_G@#*f+NrgRBN!MC!XpT%E}f0Dv%35^V!h+Wq=>ov7X&R#+3!F~hcOPL`E zUI$9!HE7tZjf&Jb*6m;jx?Tq#mWIpcItBF|hH=wEK@KQ77(E9FX-KQIUa5w=dQsV` z+$0E$ioIRpotp*6r4_4{+ymDbq3GzAdXbWX0&j(7_dKMo*S(5i3M~cucXtY7)sVD4 z@La7ngdW#cvjgX|nVZLJn}_lG{SsJ$D-K=%3b=@{g)8%1L z;>FVQ@CLKwFZN&LCJ+}vW3`-g)~W18YNcxg`w<6-e2DQ4d76LsNJ@28)-P5i)$j6r zPmBV!5rkmfX@out9=~1ee0#R}FR!)<7#(Qh5u~3@sdT5+NI!%f0;AhFW|^T2S7H@y z<${1pLQ+fQ)`h@os+>P%QmI8y-2w&g8>k}fvxBFm>l@oT`>==55FXlbRlc!1Yy6l5kO+k1%#o1;ALpv{de#?S{t4zZOk9P2UH#>wj^qu?aSFxlh1Ie;Qy8KF@01 zHgsxxZy^cduw_Qd2@Ki`L@ zd1T)Pz@z*>*7H{C;+q!JQ`k@Us+E|{Be%vJMlh1C%b*^_Cal`O*%bQTR=ql>+ote+ zCDdw&yE_;^o<7*WfAxGezgt5$XT#0${e(yqNg})I*o2&h!RF94?Y)iK?oVqYEH0!$ z-SM^EtF^P1)JC19$!WQ7d0M)sEK4j55cKCDjru&R-f!4fby)USShMAYRcWvSFo#?T zt_j`U402eyXvsIZ=E~WAhTi4hv(a~4TD8i-J*aA}R02Zc<)7I)wb1(;A7j~srb;4G zb7Tk)0em^K`atfG9{TktH2C!}81Q)v4+RIqwjfnjDXL5;a6-{}xO>K}N0-ZS^WLy7 zd|}GJMghwK@xlZa<7HGiHTX)ji}6q2=$%?2)7u%!w1Ud1;)SaNNA%YFPVr7Np(KR3 zo6uAJSEWDEX4AjW_x~CF-tz9bhFAJOP0LKchTh`?&xN#r={OI7t$4`u%#MxCyj8R? zZ1N1Iapd++K+DoF4$3OZ5L=zh<-h?9e@pPD`;Br>Wafh?+m*mBSic)FEP@)+9Z7OeS2=9}S@ zC&0q{gW(vfQ;Hyo1@YN4wa9{!zdHIQ31p^Mt11(6u(Th--_ZZX51@M}Nr%)M_VLN+ z-1&*n(gJ^4U_UoUgCOHH%OD9owu}Gqd40Jnu92V+yr1lVEn@}!t%%1&b@D6TbS*Md z5La%z0HDYNLB-tRUp5vP{(-;9$O%D?=3ns4U4FNas~#jSQ2=Z$kQAYH^LOaCzaGzU z6r;^0CRAJ0Z<#KCRI8}&81f51Ag{bZsqie=GDUFVJLfJ;7ALndtP-J>2}DT356(dX z`&^0~SK-id$E5cfz@4V-vdEnl)T*ZfrV=a7A^_@uq%jx)SRqU!NGpn_QChQFe5Iv< zi6Hb$lY5E6*lLPe`U}BR^Ii(v{8t=*Z5fecI)93tT1`!SSJ30rda;KO-$fxckWL}K zoghZ7p1OHfnD_SnPf>Vb_2Mm^hhD>@N7bg?@qyFNG{bjzl&Q}ir!p}L%7QU&z)%5* zQG-8(l%;N2>-WRQY>=-^HA0nO2T>U$QtEiGM2`hEk9sX${{81f@)=nw zfJBS8YLzc`jp%0yBIK1M{1WKo^A+M#`S}6~1J)XUo5yF~B0N)k(_E1HkN)3f(3qo3 z7lUZ{gp~X_f8(XqLpYLO0!oeVO8^@mx<^QHFS8ON1W-? zl+!6Sk`#yK8KhxDk`scpIaO;lh-91HA0mDK_re#2&Xx0VHAi3@qb=HqGU$#h`>0F379XOdb50F8w5T z<4-zm&i3M)M66w#UUqI9?qTFMYbZ+Qr^+dh3XqbSC(Un-Un%0@uxJ-hrFu+#pAix1 z;~|46k~}~U%;sCJy*2l}krUVBlZ(Hs+u61A?7H-?cEGzWUZ6Bz#|QLu<|)-$12bEz zo3oy?ozQhVUWw#9?2smP9^?79wwhQHun+~Uj>*`D9ro_v1mYjGTpgY|D#e5{UG@|T zsOL98g=mpuG7HDI8-nh%)+pz&OUm}ru^@TLa4$<@C1RC7aPhudJNjRUB|gLJ-TcC0 zJU3EAL=^6G+AYSRV4A*sGYd04tX&Heo2%Vy%5sZy2}2xO#3KC&15>5el#=Viqc(H+ zlgI2AOougM4NSf1QKUr<-KTQb`=z|iMTX6?ooUih3eSCtc;q=?U^68^UIH;~V0t{o zK)=c&H9dLlMCFO%N?_)v9K)!u_cvGrtL|gs5>3)Dydr-F_HLc`S}^qSvY2{s zibJJd>tYBOD4j}d&1W^l?dq+?C}{Ib$EP~~PR7YiH_7XZuvsX+sdA9j12v4J>y5zF zP7OP-MpX%T7Gkj6h30>sI4AtQ6;7J3UTFgN<|&L)%2M+IQjV+uy9wv!`3emyb>TP@ z>twS$yn^}zfAdEe=eI9}mQi-0x0^wesf3UMVvv0l#R>kcna?0@Bgt!5CW6`amE&b@ zqNmp5Gr$q~wLtxI$Li0(nqlIYJ^NN-uufB2`Bv6FzXvn2kFuK!U{P4vDFQ@QDam9g z4qjbhhK8vY{$+tz3qAz~Cn%ZKpVskR8-K-U$ty+Vl%|#_rAUxi1{M@ta{56%R@E5# z-wmdqfbsq1U`TPnGf)79a}?KptZCk14-l6i{frC|f<=?37KQ43u&Yp%KG+B;AL7z!z+ zQYCZ+COh62w`beV*Dg#!IyxR;@27_-nKqg+)_NMRZ#BJRq(ET|Z0Qu1TjU&>*%m0kq30fS?O*$z*V>h-Il0PN-)`UJesrRn4asHiw_@|D-lekG34zL2)D<1L5ZFcV zu%A7YDj=Ej$KMkB%r5l2W&d0Wt26L&U$5#79G9ije0@+|$yZT)*Iv}?3qa<93Xn!% zo!L^VH1-Z|74O5{)+wlS=9|Nl;7cKA!dBaqM>zL3Th#%mv|OE*F&7idilYFK%DRq5 zLFnQyu%~6|O83gUXSn&dDCD0VY{(Lfs_dzR1~Zuv6kJLb!A484H-WtmojgZb+oxu%t?c?2hvMrx1CE`eo@e%l zbhU#z;KpSsG1l#bH6wK@?%$ZD5VH4*6V$QzRgs2r~ zW-X`i?FR0S?=)VxVf(cGBEX_Mubd)`*$!7rL05zn^_}m!2`t5D2IuV}zfpmud;BZ~ zNDBH+eEnq=#-tQIH9h6tsw!$ob$f^8e=u{U#*`ntDR&5_LY6VVwUvGXVD#{GXGtlINQj$StjleUHTEWU7(j)5yZXb8TKF{`@VYjO88j+-z@DLM z_amrq;9T-BqVrh@dE>pXeS8CH#7$Ys*FgC4WzceBvCcxuLtg`92Hr0-OYbg335w%n zK|CB*GshIhBx!J3r97?&S-BwoP!T2IP%;JG03_zUxZM`TV>QqJP{ z#h6d6gx+8oU*ze1DVr~h#~&4`aoIhbxV)59uMw?N;;e4E^Sl1?@G1oT0MmP{O-U(- zM~AWCwp#4Get~TYScjso{sko2?8Qs0#aW3cT?h1;u9VK4Jq>7zAH7#HvL+} zy)B{9bB}l>g-kdk!Uvc;##Y-B7EL|_W0&`W_|7Wi=5zPm)_VLZc39^%uN)>((zjr;%_S zSlTb166qZ>AWkpZe4l!G4ctY`OW8i0#HLoZ4#L{btI5(*WyW7YcOmE=7w@fSnc`t( zGacK!*gZMI5#JMc3`{rj&4cY_OjmO#pZETYrmOUqUVw-j+R3H>Qvk(O$a{U*St9)C z2Z2B<1L-^-2d9Y62G{*t{Sd_>^pj#wS5?9<1}k{7I<{@UTI>b10^mj<=VuoCW zIj5i->Azp~r}>bFqOBRM?585GPzybNy8%b+oq)a?QYiPhpAl)kpX)wcP#JSuG#Fx` zp*-ZPvu(_PLVY5!Y^EQpu5YB47^r}}D^|O0-`hDYMEv~C$ak2;-6aNiSSUUE1*MrM zTN!djqLv|?&I3hBAZ0+IAE1X<5Q|%iVk~MPaQ9d!nssKhd$S&ocxY-qz9(u~IR3fs z;CS}o7d>V(?{0TKNR|`uCIRM45?&ft#{Bfn`HXWuPrdAiamP8E`=~l4blf=)AV>GG z6R;M@8vQ*Ln zfgygQMG!>5qX3;3Tw+nMA;<5w|J)w^{wFNk`41|pq|M2KTGf(B%IxOx!J4U< z6nXQGSaR2cV!(|nc6ePgH{zz<$ctVS@mBV$3ROCMQ3NRc`#NVL`JG%!jc!*~%&Z^| z8`f3UsfE7R;E21tDs%sS(CAKJjS=J@l;r?!m*UEg8_cCj8Icxp<0WhSe1sQ<`oaG~ zUjP2`Gu<$safAGZ$O_j&p+Ka<3u1`mXkG>UZ@UISJhd4u#ZP~@w+5ymN7 zns3iwosnSuM`J7FL9uN5xzP7|nSlvAujWS|C-^o_C%AcSAqCa`VLtJCOrr z6@Ngsop20D!~9#E0dDT4J!x9wnLT*j#3KDtm!5$`g^?Kog_d4}ctC_ZPcaK8KTg$2 z^1+wID8NSUu^g=J1nu1)llKNLK-_5nnCpNasneSZv}rI;SnF3c8VP6G8?B4R*WZl5 zf<}Q61VxOX6;5!mY@Nj*=}K)lVq z&L^uDtX<_^+Z?^pF_J5mG=B{sQRtb4Aw3`=SV9@>TR|8uSz{`J0++;7Ci(#6GO%9_ z*%FJ78Eo2)DVjSRJT|6rUTc#v+0ED!B4pZFZ*mobHgqq6guxBj8Ov8TZ9F9guGa1q zdt*K&ugV#104c}a_dC_XGG=9b4J0=9c7h|QK?)Jd0pVQ$Gyx`MA}?1bIl)@YqnE|= zE;_Xq~xOtnv-G-9hF-;=MDi<}zz6pWIxIgUz^i_2CG)yp)U4R{4UQ1bH*+^BJmLNZ>! zZIiZ**0zS@C4Wq6G&UZZcG<9k3+QFcb!a&HJhnE39j z-q1Btq3UC8geawfWWA4i^J;mtU{_s=vjmG4&%~N~=5_7PGgF4`*_rca`-?`MP&0~d z5%|%SG2x!OUP~!r=C!$&{k*|YaY+7BE2*W_7-%9^9~ zoqua2NbjUm(7B%M>!%>^#R2YGsqW`W>dbNSL+7cK@aNcniv$iOQEqF283-XzC~uzwV{!bdIrS!{$>FAKBa#wn)niwh<2xXbqc z$8f3S&)inc!_M~m`sRk~05cE~2QF{rpIoy!bu!^Thoap;`Xh0J#G*F}C{=9WqwxrY z*-^?Dq%ZgOBDU?memML>%lUyBWF6ucjXp0e4EwEjWBrTGI^lUrVYlBU>N@TQM;Mja zbut(92O;hQ=JetR1I_baSE6I0|8agwAlfb1m)LgBry0|{eRv%`)vHLCWaHN-7!Y!a zg5|_7*an$u&bg#vPbr4mjhhez`XNO5@Z#QZGa?3L(oMsLrw7lQ*WajWbwnCs;zw!= z>?-Zu)?qp+Pl#UROK(pOZx=z31;v`mZ@^Jr-%l2$-Ov2hRKL<-4DySy-WMPZmwaLS zS8bL44-MDhAT03}erl5rFrnePX{(2AJDQ7%>C~|#RjE4%attKGS=iow3i@E9iY~>Z z)g8?9MG7>b`$<@{>D}nI-x#T}WL=SjR~VVX^U{ouLAUZi49#b_+@O=~zmXCs5`g&` z&VfOqi45o1{0^hE%C)J5^7nuP(gluLxE`K<{nBI(0fPRyK!JZZSgVYITQM4@=jYw< z!X`QE&G%-KM$SR2GMGeBy(NAY-rdTG!r=kv_QORQ5^<`r-D!hVTJ*#8b3%i^l3D2< zGl=jhTS^B4#JfP6*JFvs-JD<3Qckt1EQ;Zq&RkG{d}wbI@6eiK?FFG@?SplCZq$x4 z1JTaUhLM181Nl#2@W62`&|PZ)3_4k4%zWYns#q(A1_)ge7^j~X4;fBC2De~KS%BBr zf0d!Bxu_i);OaL|(5*5uRUv&6ktEE`Gbask~baiK!GSzMk>nF4Mf4N>hd+oj3w@|&Xl#>47am1gP+}SMK@6l)B6VazH2%mtU*|&7yz{2o4)(E}2 z{o*;vJ)2K*M!?1IN$uQ_0}SEw7x#NZY(M^rq(A1UldEZ`og2n^2;avVRrrC3G)~3e z2rbgM@m{OZ?6&@~g2-b~U@&O#z;66o(B8{$$5FPuk4FWnWhd2g7nJ9-Zt$pVfXgd` zMI7#KR5vZ?P|R=`8n)pk2<2+&m=uESC|CEg7S>-%r`E}_KMtDs@Y~Tt>UG-*v}2!R zJI@wrc9loPOp|~s(!En0p5YO=wCX8iw>wV0ssAGCR^E%l27q1s-hqWV5lZQY6*-#I zMReRPBBwhkx7FO=P@sVr4|+1wvqwx6@>t{wu5f|jZGOKwd%-pkn)0ZHkxV@oIwNET zf6(5s9Z{WjXWxIbU@Y z8$Flxwto=Vk(6xqSLX(kx`*F3LUU~d03|6#L7yp{CHe*6zSR%=EBcT0^giVZEf1=4 zfSXh?HY3%)rlDU3wYD+gRwZWR5|@Mhm%mFd3TN!%#hH*yStS+OeD8&lg#f1 z0GnO0SQ3#p^JeATD<7m>?}+9x=*&F_KM;C6h+v~7%@VW{R9YVh(lK1I$nRV6@amxF z^IH;T87f} zhB-GF68ne+54$(O`=!v2Ol`UH+oFxIwVV^V+FpN(0kBIiK-xb!@ZiG*KHoiC)%yKq zZ5msSE0=}-3|TXR*f8MSy!%7qL^f^vrK#Gj7M_w+!<`Qg)*q<3Sg!swXq2zb`Bgd7 z+Xi$0*5paqkzbuDw0+|@Abw$!*?qz`?pt1oAQ8E`3x+F;6hH)sPewGb4FkUG`Cx^W zcG!Y=OEjCn!{Rl!)`B0VFVl4=9+tVHILm|I=I@u&^6=gUkvm8>f{H!p;oH%Uc-twy zDx2B;m4XT+axun11a?i@Bq-HSFY4@BGT#|!HO+jnl;&jg&82rpZIW8NZ)-0Le3y_w zEp+bs*aI?Rcn$bLpz@-ycFC-u(VaMpS`XV6zirzw88T3-s?~d1F6k8s9mMGmEX5nY zD&LsB3gv&U7i6#*{7N8tV*`OCA$~zT;H|32=*_(?B zanv!7epNmUBRgUX+9G)_R2_#V2E0CAgPyKeo8amlK8gHF&TC2%AG z^{Rm{@TPR%Y6Fn@F8;V)xlYR#ms_E8pE??t4`@OZ;OUD4>wXi{C$ow1ie-Oe(a30EwiXy)2~msa)(cnPTRKK zCD@E05OL_O5efSlEYOXMKSYWT`%|to{+=(pAthI9?VD%SbehS5Dt0M4KEYTo|rg_=?AfS6hYkRXN6Br^TiTOE+)YjbsLE6{D4b|JQl04I;x! z8fIOJPVF_mu)AJ9DoWg1s&|TPtOg8ED zHYu}+)a1jg)N4C$aCfRi2bM=HS4J{VoR7a->~aFLG^!eYhBcN7LXPDBc*V}mjjwBE z^;!IPffZ8Y!nz#nOexsfzF={4qgxNnwMM|UNBwzTcs`6BK%?-mMWh0Q84@4HyuVa_ z42m;u;^Ef|iyZ_y%f{F2Jhy0m z9V+m!dF!QWCEUsbn;sZM@_xf*aKyg`ClyL0sq-dRuRAT28+)w=X5U|(e7zCz+P5B_ zKVF(ic{tD==!r|3L#b5o%0Q6`{D%c7ZNzVvxOWI9)tF{+l5! zzWJs={-ehyD!!8%<-nbeQjWz&5j6^$N(TmKAY>i)Xu62FxHz|fK({+J z)ELw`t7W6`&Rm*DQbez9X}G|@HJs9g&-R0x9rMSL*Tu85?hZ_9T@^Jw z)Xb|PhsyQc>yWP6W0W0}m`$W~ny^YUCnp|PTY%ogzeLkRI@LhjASJe{L~_K9SKM)n z=q@ch;FY>Jee9?^PdKmHd0F}U?&;1h;;h>U+XTL(l$B#(PLn$KL+jajg2w87s$wd( z5)9qlEBLD?j)OEua$sN2hYuyp*}2=laOHRU2x~uCFS7$3RAf(28hPRMPgW-UM)$>F zx$xE3&4a~D60xevFP&wf`K&LiRr4WAVxyBQ(>S((`z0w7CXu^6eY7K7HBgiBQOMbBKTa|-L zs8Ofwgg@=R09Uw_5m;4n7!_yFYxuNUdo<1a<4ry&T^V^f$Oy=1;hVjR5g(V2O>8dS zya`qruzx5(+e*1%1ku(wsdlbSzu$z)O-HnIiy!>u#Fdcq5rPA&S0!0h1{MM(u3{@P z4r>9-J?8c6KKLzk@)LZGxY^Bfn1}pwx1$t9Td?i{A9tZ`WS~{9$5;`T3vI@t6g@hr z0R+-InLPW9PNyK*T}prgL9r)1?Wr(xRzwI~_;#y~4e zwe=#rgbI~(x3SC zA7y~ViqS5jo7-_=JIDT5qe|-@3-MRF_0=n^7Gm+nu?73qBPF`~`iwSIknHQf?~t5* zAU8NaE>p5mxDpfPe@D(Gu{r@s|Y_)8FWI z^6tLVKjet$uuQ-7+KB>kRpiXILVwW5TbOwhTgpT?0X~>ngI}mP0j1jkRQ9!Gi$xN7 zcBP@9B%V+El6ouEb6HK7Qt6cYaxu5ndZQjV8K3qqiLE`fXvq{qvpupc=4{VI1D!&Ivtg z0Yvd$?Q&ld8=~+%n_6FWQp>`Vt+V|ojW$lR9yJ);y>f<;xe}<&Vk0PfT6)5TXgiks zYnnh?g#E2#ftu`j=eqx^kWZq59JgKJId{Bf{d~ug-!TYDNm=~A95Hujf`Go7Q}H(i z=$}rUYLdGU$Mg)L4{%B!QD2{P=+JQ?YUD*~SE>eq9F7`wtUb7Cx#vWJbowL5({bLC z1vgZ-JFC@!Bs5xoehaD}?Vq=uBqDkT51;jIPp4rM8O&sIx4|erIEoGaFNm&+Id<{Q zgR;V^g)-8yb~%&&cX-lDE|BzaB#jmuu?**9R5_kuk9U1i6$z`iDWu44A1~gFpJpvl zT=CmZW&K3@*EO_jnPJt?*wc`gdYZ7|_*S8_~ydlijFzo59rb)#ac z;m!;f7qPABNp$id~yX8|s02DBVyDJo2HOcQ_UcK=CK!~VRTG+W)V)K{Ql zhPh%coss-KdEB>Tpt%Di*WPlf+l-okS;!s{KLKeNf@qua`{etzoyAG$ku1c8onT-l z+a>3RO~>&$5zc)hoo?#8>7`8_`i5mde+)y+y%>@k%BF=b`huya{Mf(hJhK@Rt5E0B zE@is`WLE1_3BpeJhfAF!_F}A;mn$0PWtMeyMsc%abO7809Bm$;BEHpf3!j}KU^}sU zLfyM7zR6fPb%n!hKtNu%3~)A z#U5Wrmt#}xjH`;s^)ut`{J$!br?;wwy*pp=4b?pH+(gSUh}3BS<+kblB~v=K?kMcE zrWeuc#1e?|t^)-IrJ3Y%GgY63dcs&g-8|rtf?95INr{>sitZn7{vK(KjY6!~eSU)2fPx zmw)gMPp3f7T-9wLY248VWg--K$J(&?0)$rn?Hd#UT`w)$0-CnCf1hU@`~LG4e~0(7 zlWXwU_&6&x6?6?`)feXlM0JW+a9||go`E2ok0vIs8oE}byb*~SC3Z~333@4v}W?rq1kKlIuG%JdQTMYM{Ry?^q7z#Wb9Od6A>=0zsYS!TO;*l=09hLn3pQ%G*wIeg1T4#N3 z%2Y?VL?UA}6m47kD_$JFiy6u&lr#F`6gXzE`mXZ&$mf5*oDoJ21*CC6Iz~60SX|yc zY1k!<%n{+Ln_~9$KHE-rY?n%=h~vH+suEZHpyZm(B4bgRgT?Y9PeAGe20N^NlFHPz zR~|U*GY?kO60^FkJGESZ9CB+pzjQlH*Xv33g3+1d%b#C%GI<4sJSg{F@3>u`0q9?5 zyHl}^K+s`F>@V4)l#?*+Ep2L>#YClL0G81nNS7&wVM}mPq*i=z1=+y2mv21nQelOk7UEji1*|c8VH&{@8+OOG)eH{1$#=uRpres+v(l8mRI6^(~g8 zatCH(?szzrNIOfq;RxmD%7lRL?i>>`26Z3So1ZSdxP%|d%H6B{QxJ>hDm(T2Fs)%{ z^8Dy_uU~t6r}$D0wY~5FPgUJ#{?6K+48!L>Kunnpg)+Nh|s3dT~uriyU@`*e%x@(_ER|MlfCQ%H_Pt z(O`So)?*>=3aMmv=pMxjY=E78<_55;>ApoiyN-_gt8OY|zB4yq)y|8C_13^j#RKhy z{4j&<#$;zB!6>&WFCJygo$VB&h0odXdoAJJ{A!duxMU400zF6$o=0lOsLhV#yVgva zi`pzF)>y+FvjnwHNKg-~E;nH^b!mqj7p>3Vc%5vg;e%InIPzYv--VO$FTA65NC$d$ zp4~;(q*S2sAv*V)lZH1bnu7)$Igaw}bL~^}aRefGf;QIP4-Z!&vy9|e=MpmB)20xK z07f9Z=LL)oBAQ*_QLQ-BNr)1B2ZM0`Oda>DJP@ROF-qV{tXi`oFcJmcDRZ+Um!Wpr zBU28tbfLD9;Hn@A^#%{UQ0;B&Lv;_u-GkqTfT0DljH2EYMf;9PKBH>C49bwyP1Oma z$yHQUopvJHqt)nq7GvY^NVk>!uZpyHC1m!K$1LV2Q{et{8*rOHL$tm;wK+~x=yNo6nwPAgzEoynL}vi`%UG4mF^~NpAK06K6n~B2ri<9PF}2(h62`geI*qs z^*BSdTiliYW&FMe(#=&iS{Z4Ecz#T~9}f{zS+dzgW}8`0-u2J1EFE6Vr_)w}6<~CM z95>&%s2JZu+RXJZaTHt3Wt)nxRv!3Msle?Lm#od8WEVg1j0fDhlRTbVhJ(FIKuhlY3{d+w&gQ757<~ZY6SQ&IrT3wh3|#aXnZc`o#E2{L4y!x%By20dA0}hehiOGop>ZnmYK$}2Pk|{ z_Z>6C8LpQ^cJp&{=SOb8Ma(eF*zXrF;cBdNah1+INu5^c%K_Z%Ag?#bYEl3Kl>zbM zMX74WBUEN@`N86aZR6I!{Px>POt*wIg5FRud68w6`an>JBKN=nhoBqmO0%G` zb#wM=>VE$4YwI1c#?!ybhL`fqGSLn%pPoF=)1BZp!tMoY^eU8$@umbv5kqE1g&Jx(f1TO1+?n z7@3-Gw}!d;U7#J(iHx1mUOCeSOx^e|^RipZ3;O19SOR8IK6G>X9W{KFBAGdU@ySss z*uE1>=g-Jns!8i_$C3|nUKTVnIZ+*KLOFoY{T!W~#u^Zd@~({^+5JM{Z5Ll;yc<6D z?It&`a;KfJcDYd}Z!1uYsM9#o&R-t{YmM8LE)SG*0OKt~a-quuNJgS^IkJ@?HZKis z$jH}k9209)x13$`s&MXZI7hl*U9EiL@r%y}opA21PkEHGK_Gn3KJZ-_tJN2})pDRH zPQ!u(D{eha$!;UJ7f6V9(m=pZ(GPUm3(G9kq)xMTnh0+s%WSlaiQ>Bx4tFUV&8X42 z-YN05c1@!*SmpeVo_SOz_hcQJEBUs;{xAEX*usNjB=v_zmvw`Mev|{*Hl)$PyDk`5 zfwLhX|Cp#a%QlM4DgEqo%kp8}@}^z!I^Q@7!JC#h8PE_?+#$kp@6Ptuj5Iypw4d6Y zTaE)xBWbV7%MtT=l2UuNe4}MLtsC9sKubP+MbFOUG~L{pG2K)ZQ9iz^?Q_&;G0lzw zgTOAi+t#WC?KSE{ft2cFuvKp0M*Rx<8qIkkI$#gnK>b9g;NhVBA^iJu!IPBn073;P zI_tvV{8+ z{eT+9*hHQWzI*d_#&uBo*Ib$euHORj#T^$_CS)^Aj?Wuc#ui&|5fP|x#slCMzWVvz z7&+XblJOoW%ZzLNQrskzKaU*uOQHBdIX0c(78dSw-Py9|q<_lM#cGdPMlpx{d&z@Zqe3X!7^4UkGPM`7412rH27@X>6$^ApEd8-ss5C0dwQM8UkpGWfAZdBJ( z>O~<9V88Su^rT)@mGV=|&QdA?0o`%q=A;_zU_ZqOzRj@?_lMv5!tvm-(x^D2)4S*8 zKtJRDeDbmitL`05>eHLTJxEuk9@_vkYB6UmEe!h)?0oE}BwMjur^lw$3lE-b(>9oQ zY2*gR2hK0Obxzy54noM(A|E@i?MbE{mjL#ZD1X4q&;|dUzvuYPKZP=N8+Cdz#E|dy zKMK|+I_NYD-T^GgZWf_GEWI-l9sEWDGyQhWwWXQKM%SD3$(=vu7GzE(<<4iAJwF0$ z;`cEWa`-V9wYClbZ;k^4GJLz>6Du? zn~LnJ_6uLka`jlzb?G{*;tvYExEkcIoCwPgpTV~&7pFR)6~1zFbM@7;jo7ypwCZ4c zisghq@1H?A_6*7jLP)Qp)*$F`d@LncQFjz^{V=&r;Y{QkzZm%~`A4U+@3ygAr9){Y zOc@ch>=C*J7;Yy=$GH3=6v`%aFdqezBT#+*_nv5V`aj42xsiTjpZ<)>al`oo^2fGU zWznSDGbA`G>$24LubT1qem3iEpH}!dYs}eDY~mU|dhwfQSX^i2@!-a$VBHK(FW6)O z?u&iUEiP$(GwX0kZH5%zs1`OsGjQV5H7MNSfCuQb043MFJ^xvViTZD3o}TRC=VLpNA__O9GWz&U zii+@wIdu(v0a|_2O*q0#p!xJ{j=%9U`0t2Ie~ADv-mPbjhTV!Afv7AKbtZyeLD0Yn z*4tF+y*s|M2vMEhYmm@YGRya|D!=8`qVaI|_@6 z|EC4`@x#l6E+9V3PdS92{Ec>i`{tvTYhlC)%*W09W(lVT47PICmuU2)_RBo@3@0B6 z+c*t&eastTICzpK^H6og)IVsjZZ#1L%HAQR7Tg?hSi99lLD@*l%IgyW%~6j$Eld5% zia$>10!Au4uDnwB>BO5U9(Y-8)gTD3OQ!agJ$I)zP!*=Wt<&w{<(c%d znS`Mt=9_$wI%lO_w$tcY+g@MPGS0B6WM8vpExAZyvzd);JDpwVw4pg7^Q|X2!=xan z+Qab-kn7nT|8Ev;e(Ew5^#7ckZE5uJNnNLX<)d*au}n@v5H&G3j$GAnst&t`^9637Y5Iu#$J(e(XZ3}iu3xUxrfT91-$#eQ3R?TS zY&vzcB8r*(u&NICk;W;uFlPjiK89&%eaP9f=@#D3Wd$Vv(4+jAyWk#=xKDPZ6hd>39 z{{Dp`#RWmvC($2R^ORQVv%v5TpzW<0)7H?pz^haJ-8U5%l}3l;f!ah{o&frJ^jWa*;jfH#gl!Vbfu(`lr%4xwyIeII-p>DW5(?XI1Y!l)68C z7Nuf4?_vpQ_zfC$dRv62CXtiZH`XmbXbzj$Ox)-j){h#=o&U+q=Ic}@fb(;c(1jW0B~bk>$L??$8SN~dpkQwWqkmqU7y zmd4gv6Jp<(Q!yFEYm{}9tk6hVD^I;JnYo&DO}wP zHz|FvM)1r>OXu|)W_V}zgN8n~1X}v5_$K2O-Q{Iouy0fyhpMv&OGeSQ_ELJl^fDd) z@9zn|-%FVerbFAq<{d^ebh{fCAIT%boTO?oWR#TMA?QKR!@rxDe&XX{EvRj|a96Zp zX}XzJCkXdP6lq>$#=yXqo9YK2bQ1>i+VFQT@&yQp=vlC#jn8tmWw-b|w2;a_BTQL( zDBFIcxm9fyMlpES%a<>ivOva01M%MzKC5;wixU?V2kalCr*OCO#=Wly|LJ4%GNsSw zDusP6uAdbjVrt+gZEjNK0pB!GVo*X!H$gpKIHH&`<#8f59a7IPsE)Tys}DZ*%#b!* zegENp+16_5QpM?4AmTdM1G!P&vI;Vs@bMD-RkdPU%K#tYuDM( zH~F{t{Ri=2KJ&D5>}Jd2!i+)j1oG0+j6gODzS3!3wVRTp-31v!^7-!G^vR&u9_~kN zd2Qn4tZCSqFax1YKXqV+=Z>P8oB`O${fd8gvdugACO(75xmqjwRS_SmTu{^3s&6|LVmU2)~a_y8+5$9o9I{$Tu#mT%`I9jb z!I5{V!0@I!jh?X$Y`@AXslyEfXP-wT`Ks4g9rwMW6m?k)rE6EBl;v>C(`t5KBK7jU zte4o9cY+#>_H=_r!4VDDS&#N#fL8SWw&uJhM|&8<>^vdJco|dqHdG3U|5=I{rE*NJ zP5*9ReH9c9Im0X0QWifD){Aixu&F1y268SSIKm0;z(Up!c z;5`3jE*ad;8O!*%v!jrgS@y-_6zN}3BYyJlgI%YFQBjCJIs84}*ERM-NmbmQp7E!3 zSs0BxmkLw6w%O-hl!{>O9F6^v1^iB%M%?G@*{U~=eFu9+Quc2T9=kY3&m2rYf?dB~ zG2n$y_g7vV>CFT^$UWFKQ{CClq@c#c&E-&hlt@dt(b1eT9qn{ThnqhSmE6kKQz~jw z%bYqC5LiqYP=56XHB>I_yZSskj4?Co z$%;?eF&jgg**QGMu-yIYjQt{`Eq*9B7x-G6xxxCi;&{pRH|NUSZYdR*d#PI2t$G67 zfOoM(=~q%e_BU%_1tU1R`!%KF5m;J*7YB~p2)R9Y5Y6Vhc;C>TxK-!9chDgK>sA%@ zHl+`s&q5-f$~5d+E&cixd#j#|f2Nvt-xQb_R(-1L-j#eNg+ap}`AzqO;vP)clyul$ z6f^d;=_Mh0x6~W%3j5n&otAbS-ow?euV`8lgD6&^g#stS=0;`H>ro=Esim(owwA|N zUdTg6iJUxoPaYd7ao@e$GxEtzs_@Li2Y$Odg6-90em?YJ34Hr=+QKv!IIT-@Zqoes z2M5xsV7=gO?(AdS%eOV&VJ4bQcZaDm9u{U(1Ls1F01Q{=P)ZyfgP)m(0TP@FQpbH- z_PkDSqu6kj(>6GR;AiD>m{@j~Xjf;?;zr3}JE9)>+&@rJ7n3GcFo>V#+Adg~$T(Nx zd0p(8qPlo_RDs>YH()}AX#QGYVhcT;JRGIKBN_VL~V7c z`Lt~QJ1pj;Q%2998W#e?&_2M$bl>jHQJW6`wW+ZbzFc&F8_S>Exwpr>WY(bk4O~3^ z(CUlUX<~wabnzf+(V#+SoV?;_$#0w&B;gT}H=*v~rP3YyYulHBc!gLu?-qX2=KOS{4nz4LCg(kb|b;3xC|YHPXt}oN7whuZg}TVR#$mTdR*t`G7XSNv+qhaemOzq2Q#q=fPM^SjEqPJ4-Xj^>rVx8K*9 z75lFx(+finP=0d}CgC~Q8 zX4*sDTh1e}XFW#@@ZxswDEkeaafJ5+Zs!fRWx}@X)d{tRZm9hNVcZ4=uSE;ld%8PQ z*U$CYm0yRhqXz^c4{ zRe+@aF6Swe#YS3GR8~{kDia!b{LJA46*Y6#08O4*!f{CP7Ug+!-`%>}Od9NT6LiOY zGqy5xrSf2^Q{(U4+A$ah@s;Z{Wg zX9f-0VXEg1t&`yVv1;Xk`| zV_N>2(D1N{9We3&%3w=SnbyGkP2{d)BzMTM(B-uelnm6MA%5<*5dXJ^hL<# z(QR`vL&un5d{M$!-K*e(p=ey8R1&&U6u^Z784B z--;VDePJ?a%|OrN(L^*86u|IX^9XJSv`V+_JP|wXTF0N6{3GiLaRI9sJ&>nz$2s&^ z0LS$sfC12Q&%|U9akY_k3vf3!m%v66$`>XKE_8~y3H{Hb{&-TNa=hpJ4A8RbgY0g2 ziL6EUQ!t8B6dDP9C_hT|JRTSu%!)9H-u(Z1$RmCQX!Z0DrWkN3ahytU66RB)v;{g^ zJ4UJ)l-d}-J@pn>X_-{%F!H%h&f6}bPwhl7jgHoE7QDWDZbig?INnkGqo2l-1PU8z z@$Rlj@v@8-1O3%@qk?p5g_d*Y&gn#&-%tt(3yr&fTZol)W7jw2eOS~j$MG^#d;MbF z+~O3W=YOE)A3t=w(&SFl+LCZYTmnk{v>Q#n?n@rd+}j1?024Ww?#`WMuHfe7<@Gd& zWJ!SS>aiK?`1v$Uy4#>yL(~Kor-egp_q1;5gJ&fN`*cO-wG9JawXq)P z<#>Yo6xW?WThcHBUMo^OZeX|QS#`kgdhk;)0;xZ4c8)3tJ+r9->dbC`=nwN8L*`Yf zW6GL4*xe)m0+b-X4u-RsuBRpdSwxAbRofcKhQ_`rVirQMyR&DZ0d9O`E%#vYOHA~aYIavwR%nIl5v%f##tzx z|M<$u&CQBj*dL0gKOOYQuVK~AuVzPO^z}Toxc5OD3fjtbwEPHu(o_4!!7vdZsQ@@xik5Gc{}W%Z|R`vip}!uJyK?)epTuy zQHDu&_`}O-whY`t0;TskWk0U-*(lMUN@cu;iDPnNw<+hxI}j!+6i(=oSJz z4GO^hN1h6u_pb5Yn>RmXJZszi(fwU9xfzqaSLQE3@?nzlsGu+=^)dv%p>#~e(hwJ8^1lW4of&fY( z&EIbz*PM;|y0`}I$Zo(b+iyPS-IqGCgDNZFFs{zsx}*FE6oCJD*yX!4UJC+BU`0$} zO}cc_k~|{(de{;7FmaEwORQ2L9Xggq^hd3fgVFFa+`pxyNGplfdCJ_0$T8hR2Kgz>O0EqpKz@T(P zgS~~mpKEXRO$=tcx~r=nn(kyq2h01fwJ~5Cx~C!-6vOYBO6IlK%$s_cp|C~jnd+>j z3#iP!>)8zRbx7O8lpL^IT3=v8oJwB*0qij3^|l*SjDb($TMUoBV)$ulX7}H_CxWRz zTt{pWMM0~HarW%3ey2B!T$cfL@an_+3JV+FV4pww4kiw9ZZwNB>44&~-DT)NSqz8& z^m_5=MLh;+zZrqE1RvQLeFF{TB55dUW2%x$cVQq~d*ds1sFg6KJJYB*DS!I!#r{)gCl zU7?H|^b>c-_aGlRY7O6NZq_GB-&&}z8tZ2kXvjzz_Rtg0T?uGtANiDbVxMNf`aPiP z0Hr~BAtCRxMVHxIYfiiA8fX|;Mi?ozcXWonsc8B5Q?5L}4-F$c}d9q@T9r9l|m~H8ZC{Xj7`y=dF#Tp9*ZH@t89lAq{k=Ia#^l0XI8d3q7cqV z(xbVqXE2$3AI$bpLqR(s%n_4N04xE8L7g{qcgi*|`NN8mmzSqDV*}^)1Ro-}C@%&5 zpWePSs)?m*7d?(B;;S;qtR^axh=NQ4iVAiRL1fS%phi>(0TIGHIiN=fD2;$J2ttG~ zBoQPGLEs1n5JiRni39^;kPu=(WS+kYp6^}nx_7Pbu6uvn{*|P=tE;N_e)ita-c?D7 z99kcO>wP`@dR&j(x5HqBosFFw?WmcXZepiPb<=w6G-myvfti7^eFC|ClXZ#F2lr(u4P*zPy&Sf=MjPQk-^JRzM(6g;ggn86#;>^cP-Tkg|immaj!l?^0h+mrI#io}%rZ z#&*fG1+fF21MkPC$UW-m$yKIKttLyJc|{E)Wt+tIA77ye#BNHn13an9E@O4?X8{4n z?{0h{TCfveByvOe?$&}Pix&ifv%k>oq40)08F$;sIcPC4e%vD0B;Rp)Jb#PgQ3M1m z75Z2AHcmK47+*w3@tsM(FBC;mLJsF6V>as$jchMFu>#_9YCXp=@Mv7 z=MuXz*MLfYPtLhvIY6HOWdy1vj%DW!^-FXGW&Q#9)g{H%k%OZPA6r>q3{rvnRG^f} zVU170_#{LBk^Rrx4zOQ}aD2+Er5A;eF|OYezNP z=(>WZ8OhXn?@CwW`3XDLh;t$!8$Zb(QO5>25SNxt%vqrIJ14_H`1JUHrt%g;hOg{- z7-T0QmU)DrO%De|#t2jJlu><6#$&Lh*>)FhI-4$FhIn^%)s&+fo}oE{M$_e@@ZgWu ziLe~q`k-uQQmoVNT|(_fcP>&Ys)+drPDxkj)Ja^3$P`WH+Dw6(qA z=!CCz^75G)llh;A0qcG0Vo32*I3?>}+2%Lfejbnisy}T*`SxvC`m@Vif+fBsiZ|5L z)#uSOh(n#Va_CzXhKw><^{cXG>8ve@H`4F-FZ-iu4^eIL>47=mZphZzqUuRp{*TK~6bfB>cAf8-yfZW5)KKAJpHH1JCO!mLAyeSL2MHo6uaHZQFNmcaw1 zAVYHnDRyfB^E(kG_UDWT-;rH`%dMWDG6D|5;(L9`uWKS7ocgW4V!K5#M#h1elFkb? z#}{=jzZZL~wmLHD&TkkII|<^9BexZ{Z_kd;wiwQF?Hd4LU)v!Nc!?ew`Deu22}vQl z$nxP9C0md#m6B2BfoxUV?tfqK?6WLh*xn^LB$in}7UV3tOLUa@{f=?L$FJIGWwD!f zSSGn6)pzjb%-44)`NQVWqem{uYcivn4E#=0TT)&rxZY`a5uXFH5W&vdL@kJ7zoVmT zR;x-HFO?(sLa;00Y_jX{i`2kijmU=c7wm}Y;bWVGEg#_jg;@jlX1>avToq|P?qY84 zF%i^I8^x}SsB!!*cJodW<#$3VGxM15jh4yRAYBP!6adu!k#AD-ZB?RvSk<=Le!{y2 z;0nk#iLOHZ4_R)}`M)s#F=PyRlKw!>N8tvMh;(}}jY{DjHxTp4#9#duwO&#&`jL{n z@oge|FEYq}Tcq^$o|bep%^#On)WR{u0#x;9vMRn>?IZnolP}poecME4CV( z9KM-AtT|GtM5|Y5gyQ}3iTYIRDjQ?2waM&f=A%cr(ouF7vcf90>d~V|K`yK{nQk8p z0uQz~G*0llc%*iV(C&5Qb>bVtS|7Ct5g_mA8GmM;1_Ywn+ph{fsr z$K?F>fxxMsvj0SyVq$@We<2ey_x5$r)ll>j9C;}zcGg_CFD9(?Rff4CDmO3~-X|9$jd))7$YVG&9M-nX& zVI%r2rxG;>_0M7(eUuVQ&M>Y)i`mS0Gb2B0(nKOxU+-iM=%;5?^*%!gc4cf#{Icg1 zD;Ap~lDO9L)%kNP0Q1(rh}fsTqj?>I zvyU$gQ7sL(zUXjZ_<|WcFn_ht=PI&lH(y50T`-YCOhs71epDi|e@q|hP-v`p)qKeN zlw!s?C_OjdZ^o!P3+iV|Y_H9At)R?XAq6XsuAWC2Yvv^oPbD_HGmmQ;1L==soqh6D z%l-Cqht&~23Ordz8RfB0)rCQWbCQA6}MW#uP+_~#CPcpJnWwtsnWrsd~nWyQwX z_jt}X@7`VldU`W7?P$Zhe~4#c5Z6(2SA`khxi@sde60YCR*Y0pLgIe7%g(hg-iU#a zz9|}_TwY|(S1qZ@5WI6h1<}<);+2J~r0KyQQ>2$h>ci^=bg!2JdVp%lxP!Qmu!!rN zq$0?hGU9v`a-G-ezED~UvFf~Vs?wwNk>TDImvH#Kk3xo7>&qv^6R?AwPAi@x<@G2m z*5R`8ipyyAPPw4G^w6+=MKi4kj_%o2m@qMLWyH6PSo?EqXB_t=)o~$p`aDIC7rJ&hO+Z`;8p{7NL|USe@vJRVq`YqYw#!vLorVvj*&X+8pTSuARnZ8eqQF|7UVkW6zLM{HL z*jQoD@ZJ$TXkK!>pT+s1--3f*+;V0yM7S47&gYao>`)yprys{nyq&6F-`{t?bxVel zQ2%TMbX#GcIZ|`WVAOKll0=B#{Jn3P{!~O+$|Sf`Y=50kTAcV~)~6>=1) z>0O_PNZi&B8Zu$HN5o{iZHxm+6Oxcq*5E>yPg*a$T0E-60Cq{7Aek9miCI*6t6t_Q zEQ;eERpnnvyk9adB_0!@$~TBoB7NB#I+|g*QvLGD>0GCenXc=0j>(mX=U8uut`Qna zCwe2&NpNpDNX)H{sxFFl+)oPY^hvXe3LZo;dMrHpG(Zoc%1RjZA8zWLF?yQ!4yhndhH%~@bvI*_k6DY@`|@}v*l2BT!`biZo6=c ze`3WH#cWlf9b+l8TzI_NO!%(>x>p?0x|YlDcP`tG1UqE9P_M5P>*OCXL#7^*PL^=5 zJ8Ltva|c{Px+`cuj>{ruR8T_B`v49KAgUeTy8hJz9!Eiu@!-=V!?;J)U-er6u1jQG zw=^v9JS|_{cL+LPo7)6?f$|hFu}|IkB(6zUs8t_{JM>tu3C~#+4 z*vE10Zdd;KEkn;x#Qa4Lj+}+wAa_D~MsXdemycynz1S?!KPw3x-ZAWrj0*1Pb!xv# z5qJ^ABVr;#YFyo_`+c3u-ut9Q5+T^|;lq34YpY(}Kk@DRxf6PJow=YuN9@?fMju;* z#O4eyfb7_{zJ{j_4MXYec#fi5&dK#OP$OD76y&R(E;aXjw@K(ig@FEU=yq0|^_Jy4 z3PZg8>1ax)DHv_3~_;$QY_Xw+)TVgC~ zbJ*H>zGH5Oc*v`^6A{VwKGflo3#5tgex))_UG6MJu!V9pR(vHR_ce+Phs!LDf4MTK zL`a!0`>+?7;Sw|DAo}r_nfN@j#jH!v^3#E2onpCN{j*X=M~b{AvGxjDRz?wy0;_h?>|=WsZek9VWZS~ zRV(Z8oLILU&vQ`C7Z0aZD0Ox?}lcK_-$cD>1hJz+yqSMh8FaG8hwz;D6sD29u_J`Qt z=_7r0Tzliw8pjz${T;?YPtVs^4Ns^P0fnFgE0Ejf#b&}Lw-k6AA!SR{Q@?1UavmZf z9m)UjjkR4A?dcq3;l~vk4FHh+;ldJye>&SoE!ir0WoC;QswDVJ9?Pu!uYN=`+*)G( ze2f3az7z0~a!O@EbV~l4@hKh*Bb>KZFH7{Bwu>W-)%Yxmz+6e^jL4O5+aM|>1tjrr zbyP;ttzSl#kcpqvIO~r%**+ldhFp^_X8F#fAgWyI$#Kys(^uRQM8>4Vg%Kcbe7f*b z>z7*`1NeZS0&h2Dn&?+dTmwbH)$t`K8DSJbQixYn5!2z^KG12=k@@nUBs2K@|0~3> zdoRnCH}dxkp$hgN@%mDJCypB`)Nf&h*DTk#u=YUVH8t5RUAr7r7NAtbGT;6#hp~Fh z)z9we=vek5%)TKezol5fp(d_(iN`^08Hewtxi2I1Hg9HT#uSZ>B`Swpw7i)6BG}_` zR#uh;LAw&c`HEjr^ZbV*;mzDp(+8G}W}KG$j@Pt`?;BNCl94H6qvtVDgCIC7R@rok z2=1gJt){@19pKQd6BW?epbp9^TqC&Ou4$$*ug`^6Thul71z)hxgH&BO%Lju|9$Y-w zVDi7q0CYBOlGgaVmw%nQ;x0!%XdCeVC^`*}W0or|sz7q4;R_p??6VEedh0RoF5kM} zH`*s};;gV?%^_ryk3x<1-j^iO&DOhXH<^sjFRH>VMa;N_5{~S6u*%j8=qV*kW1N*| zSAN%L&|htsE{Cy2C_a7aPPmBRFCJ5Q;(RU)%x5g|JR)zV8Xr^x=GvOEA9KbOi_0fg zG1-7VzcsR6IrMeTj|CrY!(?=9tfCsf{F^ow7f{m|k%Isgni4yA?krFv88OjDY+2u8 z;tM8w3jT!<#+P);9xj^^pxd#?^u18SGpli4>#~}rCOK7m@DiqZKO`X2wAS4_zFl74 z=%DQ8IGu(aET(#XsrTVShnURo0#S^ed1WV-%+duizZbwu03!WjHy+dc_c4{9UV$S= zAZIddC!qSbbGcm8ct6jH(2UlA{)O4`<$X}XKI^z{8kU7)vU?CTY8JtY$9C6-k|x6x+E($c7%H*QQ(Q%br{4(s5JbGB|+b42E`L7Z* zH&$p$(AKl_{{z$9c}(S&wGO%X=G4= z2Wm2piJ(yhy6qg0g?N>lyIe2p`v$QSb713|)mrvba^P)&dK|M|17|SJGE8(0F$^r) zUh)FYH!qjTR=)3A+{znw-o9atOBr)r3~OaNWykd}%}_snRHxzXWx;H=DU)4eIV@|E zew|REPVc(gH)>jso_CF^?Q)5Vib_76asYUbXA^WjiCBQ%O*I3i6oJdRDU*KY)*A5) zX%syVFJLaG7>D?~jHOLXOsr~nW@*IlKBfj&0V1;Ec+EUK{gKF;PCw3G^Pn?9Z$NgsouZ}kF7n9v$}xw)*O+-5@zu#TW3PL4S>2}TMY$;%1TEA ze`299#H+vS>t9)>*f$kJaB)m@^x3(u_avE7jY=eUoT!s2djanUN@#-jsu4Baeta$x zN_nWhKF7!>5D0o|PKd1=Jd;bZW{w^>K*5DccjukYZGpGd<*xtuprroLp+iX*u%ibX zE@8>(SEIMHJPz$T1y?6NwYRbP8R+I)XXI(R-$ZiVJ>ix&3t{JWiTwP?JsSY23ItRZ zBkkZXjXq42oQnHvu0HkezyDtF=zSX`|Dom6 z`}cvQ(&>$5*k^Y4W}UM9`@k9;$2pn^tlQqMc%O#a4Ta=Sf`#x1b$~R`;=OlYH(-T< znC9tdY9nZ>%PIZ}lbsx^J$>I=OG8bK-sY3`oyjhX)%FJZ8!E>!Dyxo_(^Wg5tzATI z5=<%ELS8i?1)g^9E|O9l?>-4JBv~%0dCe+}zPy3O91J2d$vdb*t~x(!aEhPUo(< z_YO&a-@Wue-PBPhmxB~wY zedE-=H}c5E6AT#%2a*|XBk+>|NQZ%-gzb18B?FljFv!(7TKbf3~}#2R%GE zcn%=sdIW8wq%cv(iNS$e4mBuzr+eLC;i(53LhW<)M9=;D=L%G`T?TX3D2G|n3Ps-} zR~hWt!yWrDD34huqbvhtM8_{IMn>zvlZh9^X@WM(C@x_y|29^r+plUYzG)LRexh-h z@;oVt*#t1xHq?t8R&JSmAfWk~b~Xu4-W+G>^>ccK3~^tH5G5L9xhK=B&)J_g^5`Tn zaqJSlQ>fDR?Fkn$Xasq^k((KuU6$!MLv`dkiFEQFc#F~4!a%68G)Yb#%=Tlx^aW%uaB z{9=_v2obYRI(**Za|;H$M% zoU`O8@+#1Fr-krkUx5CWs9&?8tANXt3nadq$+j}dpjw?d)7%EW8pUe6_b1~%SA_)R z9n{^o??y=K5uE3YaV(&=aG2)Xkn8%@cW!__uEyfxhzMxW_x9~2-t>F~aKt^Zp@Pp=@O`YqmP(D5z%sUk7)i0T>WkU)DH>T0)qCm`VK}lf)9Ut)bLCj zA1I#)7`WEjMj@FdITKp#rmd|VLeQ269jR=1Ht}=Pn8_ZcUMSja8hocdMn>iW)-+De z@FUvj)y%4EU^3s(rew~Iay_xad|;;T#c>BNV9R!5loC2Q=+yr4Sw=Q9zawO6hWmK> z^xPqjq4i9*YQr=2;bKtlW1iL8 zzh4%Pk1u+gwGjZ}JPgQ~t-1=cbJwn2JBlM4J2CP}0y2V@WT)a@M4S4xc9S$A!!%;a z0??*N1-{dvaoTih;8UO-CE_0un?B*?h#>R3)b`-c> zmlc`OiJgtI9j)k8+0E-}&;V-7x||QMcwcA6yxLd1uU~5)Y6txqc!ujKodh=vr8ch% z0Wdm%Vpm5jFKJ0@xm>bI*ClA1)byGDC42talP9l(rhaq>xlzF%%`A~>Yiq-Syj2mY zk_!5->2v|BexNA<<1b|Es>p606oUQmEW6X+iG(9%rP{eogCO$XFCUzNW+t3j_ zu-PM6L9=JY%3j|kySenSA?N$WU7}yxM0Iy!$KGt6e4+)h2mkxWt5u?Rm3c^H>g6#9 zp87o{o{w&f)jnKRAYwzrhQZ`98*<-7j{K$oBX literal 0 HcmV?d00001 diff --git a/joint_trajectory_controller/doc/trajectory_replacement_past.png b/joint_trajectory_controller/doc/trajectory_replacement_past.png new file mode 100644 index 0000000000000000000000000000000000000000..a719f6fc66bd9c9dddc0a8dbfb5801df45413b23 GIT binary patch literal 74230 zcmZs@1z1#V)Gj=L3P>uAgh+#wbSvF3fFcdjF?2VIAPqye0)ljR8h~`y(9+!KNqj-E3cLP2obCG`QqG50D;%@9@26A_IXScGm zaW*k_Fk`oOvPjz#AqRn;g5;%MX?mpZ%(;6coPLKNo?~Lt1knf4D4Oom3#j!@f@}6< zXRK}VXKMFwuU#x5*DfLx6B9z^ViPdqnllSZriWgvVGmi19ADm_*5#9U+I@K&ix!-Y zKOw)jl77D3xVOD>H^1!T(1RtP-1AhKz zLgE5~VXT0$w1qG6YBq&#cY2wCv33SLf~u4*k`kH5-<8Z{ppmb-pgn?W3u{iXFz}%1 zlu|$Q-kx)JPpk%g)m8L7JG3xS0_II8pK!E{Tb`A9PbRlj?BQFi8|GF4=vimacv0G$ zIgYv7NTQ-Q!<7R37L{y8>LYvQ4&6{vs9Zx%-r(GykKrbA&k!=2tX;URB{4#d9OavS znmL-rA|*p}l`LJP*@hZ}E8I>qB@o*Lyk3|_D^dsH2ct29^aG^Z^^9;i8N;%}8Y3Bz zU(?A?mrZ2z1v#J{5PX)$3E>aMH$XQ)%R|-3VRCYm{(B8fwuucGP!<)AM(5_Nptx8+ zR=b?~Rg>8+a59Gp7E@9EceMDA)@fkxN7PBxWoYMUYyp|=YDNNTZ+BA%+Wo)s1$e$J zQ~H?jceb{gy5Jf9)5b>v3}M+Lh3JwP_uDxa4_I`?-F#n8@0l+v{1vKh&1zkjZ;-P^ z53zO-PT+u>VG)llP`6 zOEmFUX~9)ybAI}FO@(N{nsS)D&^$;jaebQRAOHPaJ*ogA!`40tY!;pb2JIG2^j|5f z>2t(2G(oK|@fNX-xH;*A93U7pVAbirUo$g~@#UP|Ga^4=QA6QDv%7DAN8`gCG*m=& zB;WIl_cop}-~TA?+fq}Rm6H0}*a4a1%2}2aVu|FRZdwHLwXJ82O|7pN@P9mClxD%2 zvubYw6TUVT`I=kI`R;d2Bv#^cW?02Sc75`me&a7P<~KRPxXMqSjMdp=%jeE=y*n^E zIy(CH?c$=0)ffG;Ar25s*ai!OVYd6D+9}PJJaWWD)#HA zVcD7J@tmSv4n|%zOVeP&)4`Q%xk9uetwB(5f2MHK2Vs}buX__68LjarigmkEUAH&p za%5-0x#<;dw_fuc@EFN!^OG`W#Fka_+p&IGN!|M3U3G#20u!>`ze|A0iE0+(QIpi= z1TO6~D!r%sx;fcPUs_~6lZOMD+1Xq-fWFxLJW;YaUCQfyzHE)qccHhp{@5E5Y4GFZ zIM^LFz628}_~@{2CW`Dg(_cJjL)yY&sZ!u-N)OtaxrA3`Y(71nmlK zE)Xx!tlb+-*kZ_k_zw`*yA|JXXn0UcQT?z9_t(Sy=pExfT6?~Cc>_O{eWbOt0dYCu0Pt0VVYJPLG&mf6u1Hz{Z8KXw4B)2!> zJoytvyBc(KwT=`*xXO%BneNM-p>sO!#h~J!+-I&Y@q!JN# zrYrRrbelpITTj{6*z_>k4et)L_WUmF!gR;Z`ZWJjd`x-CXKRPNes%f zWiNT{jc)lkSi1wT$(Wd=~d_$+Rw9&6BfgBSeFgGS;1=28{U$}wwmgA;I|GUMmNPI z4%H{RPW6cK?)ZiRthfV@YOF>Zw}8>?=&Qd(Tnw(9UJSC;NS}|DwcBJhepf z{pLg%Rb;lM8H5BdL}Cwtn!iIqan-~)nQHVgiqWW0F=!}oPtfyF_NBq`5v;j?&Csk0 zmMA8GT~B*@8jSyATQjj%Bd`GQ7^T||{}zRH6j<8+r~u-=YQwsCG4|jkhOuD52wzQ0 zrVjC|zZX^G9ckcUURzifSwD2(=OiaZCDat2#}weML7|hx*w@g8?In8(i-u*FxW?=M zy{GbrU6L5EOa=$Rml$8jIMaJZ+;|)WhDhWz8|Y9@#WYifyjD2c~xh(NL zJv}uwHFqesg?|rb06Um_F%Cqvw~M$&R6{oQcEr-d4;Mqn804D-_WQWCR6TD`af`%m zVzSNq>sOAm@;`sRI+|=)9@?7|yxYj}^y?vncGJmHxsl37Q&GATLzk-W~ML}>&92g5*S(canbUmz$ax&4-&`f=^tYTpX^h+ zM?Cmya((7vyCAkls;`Y`UC<{&@aI1} zP3&qK=@3d|SvS~o9YBn{F}_T<#y9^1n#~bPHd898#07%`O9HoX5nRj(%gELm?P*Xs zG&)H!`C&7a^zhc$5_HT4#euUtx3m@IFVL-b+{Bcghh-b0v^}Ju3lM8}Z8v*KR!+%O z=mtSwJoyi^oYbV?)n**mM|%GNDD8I|$P@-`PahMi0fh&&wi_)TjJe(8#Q$C7=9xcM z`x6LRkUkg7zoYapSV=>OdIMI0keYzPtv(|VfG>f?k|aL`o~6QdDRR1HS-k3#P5jTa za(AHA5*IuNz|`b2>7I$0qDQTnfM!> z>H!ALb2N6~rpqxu143ycFkuO7CQTY#P32Ld0{!Rj5(^tozJGr?K3$dc5x@1m5?Wqf zEkBTP>P27vHE-taRW$$4PLb2EcqznrYJB->45=hMLI6vX4ZPkTBd>*4%d%>ft%ZgX zkO>)qXhU%6IbuHvs~7k5^pc4K4P{kSh(#0C)&36Owqd7AXTVlHQMji zrq;gfW_h(QPUTOdB1fd)Yad{^%dMKpO3O`k(A_~@+u@MD`*3kN=bEYdnF2$cp5ITL zz7&4EfVP_@j@jFxIUWu>D(4pny)>}fcaZ>UQYhhSGJ)f1vWGY@91s|x9vI`CGof~Z zw9x!f?g%8_5=bP+=m@=nf;@JnEoOF^tWYvNm+tdH2Ij^!^ZJu35YejKk(Pi_HHs@X z{?@!dvjy={r6(3aRx;_|02XgJ^PMRy5iL5O4tE{bz3U9t)hCo*Z7&`>^9RucNf-$XUBBGt_>0VGyRtAC zz67_?Jv9pHVJZ@*@a%7d9&G;;2dpc)5LFEM0pn-318g-D@Dqs*975Ep943}tQ@MZe zHutQd#HR5H>_HE$QO2n$T1v-~#3%}Yt2Nww&&K~@mV)~)HcngDNkcHc+&^f0`Hc7E zaRRqZQRXr2qQU3f9tXz7Qv4r-FPgj0**pm=6L;Vh%GD|+%9J%n85AW zmhmP1ukiR9p!&g6ZfOOwGwE?Onjo?E>4G`+#c;r2K!Eu`7X8(`#KxGcc`S7ym-y`E>_&a5Me(GpOW>((1+IN`7h zWifQ0TKK=Q5sYcPmI6R%0u78*U7=gS$E-H`i4b9KAsnp*YW9vJ$ub=_h3kXWKM z6G&VTIZ!w_ZFxMtn6@jul}adEr!pprTsI#eF4pZz1 zys>ID9Rv2A{-qd7-tfyHYBfR%XTew51o|RBc<}ABIyfC-z_FJ=t(C|W=0_^=R&}*+ z`u&*0<9hC}Z2LU{j-VkzQcP?VKMWUT$}31GfuePFFdkL2<98^%wD4JY8h`%(6Z>{X z8UMeruiQQ&6tR~D>JXbTRD9{-vVehhtM<#6T})xE2D{aP?X2wrV(eC0Aa@J)!raci zX6`|~xmJB)5RiR3EnTi_sHv5Q6je@snQL;s8J3Nc>pytHYm1D6&MeGLf1{xB`nug> zc_1PJ_7xz;!+hLrdK6&)SVG`lvZ0wJpTiqM=_m;nS5u!g0A-HLgGMxJ)ZT0+Sp1>p->>2&&kBLBB2e#0 z1h6f&cc&gN8ZYhxMN;bF|E++4`bH81bCGzF{HZ;EzN~y5J(Rv_I3SF@B`0`pi8Qk4Nhu~02Smg`6AEa*_ay1VlO)J z|FsGrmv0~2-uCvcLWJQ{&4J4M^T@V(!fc+ui%fy8-sz5llomh=oLZr+kxmF z7kf?2wlJQ5QRpsXpVMMq1hm(-?bj*F6P5iZJ2?(9W{)dSFX<;aL6LF!SO_!U#9)S;K}T3-e|TYGTilN13@rwD+>J3mVo6jsy;U`wmxw7SU! z$|pde`d5IPSl1}aki@XqTlVcO^qI4zE=!32nppZ4{MOaq#)`T3O=$$H$__Dyek$MD z_a7g5)0vqyfXLFHCERxR0l@>R^VSg6sDhrT8;X-k2)I%2d21$=k?cDE^Wr@;NRCGb z?S`&K_nycjiF$z`wfZI-AheYP7=CqI05TOvng5k->>9rV)qz4#)=^3VyaTd=@a@p4 zaww&C5 z;w0?+h+ba3RPK%DLFdI`1bl8k`Ft%+xJx%7e#m61oD>N^^*I;>5Ww_+%5)NX#)zP)Ww_*RRbdiyuUMyC{p3lUfT$7&%(v-~aeQ z3Pl3%_8Y)@Xa%+wl&zxz%+HC$X&fT*^lGdpf%*f{B`~mZm{OrjKs03z_o?<1joWLA zw&;p9`C0gyH&f%SUf-eNt+gm4Vrz4+60~|NXw}Q8Q7pzZM^L4}JRCq2(zyIwA*}xM zvK`0*+{hGJ3WgPTR^JZDjUCKQ@-Ucv;Z;mJTEz4JT_YKC^jp=xaRy5K)aNjqu+Azj zg}-!(JMs6fI5Ph5;saK(umUb+38kC{@v8)S82r0 zwbiRl+Uh->8(lXaTjaitBoz^W*uLDK=c0?<8vNtFuv)MH&(xy!3_7Z;6uooEmiH55 zH1zZAocEt<4_u*t47ecSE-$<0{+k?M8pU}y*EcekCOU@q>QF+O+4s``gI#}vYxyg-92@Suof$SdT7%n zx;n;^%c7$Z?+}nzg|)O;VJtmc#4DJS1Qg^SAXqf-Ex_3VJJ?|gyRz64r5le0nhU@} zewz2CN}sdqPk~`;jbD3{s;v9TgK;QG-3gg@>4~mLPbF* zK!s9BZKxEjW^f(Ba^zlB0qj>c6AStP`aJp`4vh~$?0Bnh)nF&LzyUH*-Q;li>QuyY z2BIha?%uh+`y5I_H7H--^>y%nfc{xGLk1Aj$KRe?8=`rk`Qy-}W6wU89`>%$o5Z=$ zvp%;648PaN0LYu#i}hKnxKMq9wTc)Y4ln( zfSw3bevx8&rJ>4ocQ)hc533gPEz@W_#CaSlmC+i4@N&F70Fvp$visY^3vhs17Bk`F z?gmTQZjnBWLO`fyx84p1AbL)(D!J|G<*hGjZKvAd>7ESX^L|YO3q`J7Q4yDYts-qm zPg^NjA`MXn_*cll(VKKA8}=0v7^m^1J+i(3IvsF8<3)f2DmJJuH<;gE z>1C&Fvl{jCMAk9ZazLt7A`dw4z{~cZK#?P2^t36lPBQSlQgasIc*RR#b?dct3v1#U z`A74L9ESrd-F?2xkr8dGRm5xpd#r}t7rqGdDB$`nhb)(ffO5%=8X%YONe8+*c1IdL z?B#K0V6NL9??&hzoNsrPzxwdI_t2Gzm4(G)?IX!IIflz$k7S3k>jn1Y)FV@giMT9x z-ad$=>}_a=p3j^7X*qwf`Xz+vb|WXExeIV~EYfVc<|_s@1#%22CnJhJ$%Z|#CegXl zMO$t65F0Fi$^j-(wJB9Z)H^5EKY@a4i&~aS-rK%sRJG0jxXgdf1_JGexS_YrBWHHTJyEY?9cP_gG68TapZerBKh-_lZL9U0zvTki#;;XC7 z2j>wNPd9&*UuGP?yheoOk<7Pa3WfZNTE$GoNHW)qRjU&qbHkJJ%0S1jL`BQxdvc`= zsD6rF%0EowUufGVz*M>IFqz$gz)1qH8L?0hW($`d!!lu-AFeY&%c+ z+V%F6s&AM|i?@XUlKbRkC$0e;VH;4$^Oxz?Eu0JN_ONm%Bio?{cN>50 zN(4sO8aWYClIoe}$o#=ZPW9eUU{%{K#4DPrW#-ytb0O;sW9Hi=FcNgb<%n5(L+pWE zj^XlB&+mMWhnXpWJn^N#>63-2(M>UK}NjcB-`g3F}FzMTnql&jz{g@WbIQ%M9DiOL+H!%YPYIoaysR*OIiYBW%-X>sgjxq1(AnQjK4UrkrC4fnR^8kO_ zDocU#@|7`7R`bb7#9BNs0>GwHyMq8%b6{GTd!N+nPiTT7mCq7ZH4AgMDY8M(^ANfH zBq{7xD7ZowMt<-tc#Is~3nC;*Um2!IEmz3TJI*brKRu@TR{I^Zxhr^NZ|JE4jQjN#)vH%P9zR+nix(w$bC7W@ ziH<#GW>~1&pf-X^G5ZCLF_N;$xot2&EJDyv>0cH5EJdj^!x%_sgV9Eda0a8Yzxh*m z((Qg2FRHUo<@$FPzye@?vD3tCi07JIzW6q|Av$ryZ2CPwCytcQ!Dtp7pX2m3x5Rog zjHr)Uzk4xO@=p|Enat<2ZzQ^T$I*S0lXJZil9IZNB||zXb6#yuG;q%exkm{HJ87~5 zo$w4{x7>1lS@Ox~A_Sn!xRbuvxj1!Acnc#0IvgJfhj6kMS1U>K>;K)S~ z>R82B1U>`MlZ-%5&!a?S-+9bowTtp4;CNeZnXTf8h>G>AKfT^>8@&3eINj=Mvs!D0 zWmZte+S6m{H}r}yDSjwhUpP!rfr*{@i)+3LkNr$-YpWz zqA}L|&0xV0F>&j6z^L=hN1adD{9Lw6Y?W_WP6hKbdtQ?mkOF5Y;**k`H|yH^XYXTy z1irc2-1$xF^RO{}5=^Ol1)+z4!H@K3YR$%*_B&&WopxvN@|2c7Fl$)49(>0ZJJo!U z;YR+EO=qn4+VJvrpWEil78GXBu~yo)V5W`GIjZJ?NZkx%9C6HyS41Gv33UK^POgBGT#UTO>;n`!8o@6}MN7DsB0 zwe&0Sqs#tOu$rdSt19yqUaKFnHWTGmr`CM_7c^<&H!(OsM(8!)+>%<*WIX*MZ`PZ` zUhD)vr|sZ=aJu6I&r8?y$vEvQi~Z+TD17#_eYuBm0=J|2lhfYF7%&ewK}~z0NUiAI zKeeB3OfX}8Gt?v-o&31N=5XXIK!Fw#){WdvX~@r%x$o;J!ua%?MvlMbYz(!DQ(m2X zd7ZCvonSJv*8kjdn~hjCYv2zV_l3jVjn3dgqa$tHesf&i-6=<>o)1?`l%RRryt^u* z;`MJkvqh*%lNyQ8$Je?ewOeDMVl+b*J8u=dPJ84M^Ex^xyvD$YH^}zxKyu!9jclGw zJQIiY&CXflDNu5Z_4AK<9vDS6?7R><*-nZTK}Z}M(jHTQ#@|1y$?fNOZo0^vKeYdg zVDl}WnWbZsTg27uy1>rOv9b@O`=81|$9}VUY_s`!4hY&Bw2AG$J%#eMmB)O~&*=O;=$E(#nVE3cd>nc&gu+h% z@qVIJYPyAncQDUA7^B26I-txD*L=A6;knl`j>^@ZyVYooP0Qaz8A^h1+SPF4=2K#E z@#`JY#OlkBAF;*H^;%Ez#U1-rle*ML-b_|XqN~*3c?~oz<2FW-R?Hl~xmy<(@#r9G z7WMNH&HJx%Ta5r*33{A5ivqX%Yk z@{($v-Ki*O?MjTKz#hzIQ^@u$Hs-m0G;P%9>u+Es{|jS->(Q)lOstB+>wwXCY<$2I zSnd*E`@A=2B8QA)u0Oxx0S?b}K9w`jFx~geP7#K3d*!%7()N43E7FFCeizqMIn`n6 za{YL7#%}rZT(LX?)_0EmywAOk`@K|<=Y0OoYvt?p-g2Gc<-?gedsX3yVr}jy!BxUm zPCE)&eGeMtRK7?WV{?~{(U9Z4=D6<`>EPwV5#=(*=i9_zP{KWkT+rvsI{)AxGE>g{Rn;GmQw!?$wvM+z$N!)RvP;b*%Z(w>5Y@HPHs z5j%=*(-*-=0x(_>;njH^W@>GhRFe#PmfaVWfes|>Fng`P;b`6))9{)+RO(r9kZV!b zBiVx#fL!2f(tc=sC4To^gMUHE6X$*{fIB)~Vx$}+-N~}`yKoNkqd3)xw77>HjOtSRC$S3xZ6$pN^Zu zA6TBZUz4n#Tg|v_{fcH(&6~4JMEm{5^YW6yf6g_c;o=W@l4mj;W)o3YjJSIY0};N| z_r6)J_U-5@J5!HUCt4Dhaum805wouVNJ;_t5FbvH2R2C@`CUU_PAItEOUgzpeuh4# zYTOf=Pvy$#gx31j+4;aX^R4E-NMdh3369OWecnM&uXMA@A^%2m$I15lSV2VFCWA`L zCPOMA;f|LvPF97K9UR%9DF{S5wFf|uu=i+?G>$-K zfL#|p*)W#ZRc>33UjJq9pC)B+WoSjLl-9W-yf; zi2SfPsmh)RXy?-)&i^6Wmw(e*Q5PGC=45BTn>x|1+L-@}wL4Q2E)4!q4s`JlXOzkV z(UzmKg(@%R+#5E35irlxLfZC+4~HZ#_Zc(GbLS$b2Sv4(`*Z>-v<$jqxCf@pZ2u>D zYB+q#hZq{C=RZ<@2(bZ0&CMdXPlR~(NO2VE1~N7ie=5E-Z91xavDdOyh{j^2>%L(m z--KSn_JQ*9q&lDTCK_;(WC&58e<0(=-JqJO zvzuFX!Yq7lBYt>VmLI7VpLBep>h}XkN=)Ps$(X!1(sc=`1H47_oKr8#!g>zQ6OTri zPEp}BFm*ZIHPC05QHIQmB|PT2YUW^MO1w;z&N>--iJ}bZ@FB({>nH1Pd?SpV6z>A zOXakYum13eISwtCCxTFuvifI`X29JgwH}-BvFkmSMEZG!|JESra&ZtbS2P1qX#9}W zitAMg>%k7+a<{lGF&A<6 z8^iPN-{)e~$Rp&l9{|Ho4V!PG{V9fgkJ9*7e}!`ls1?q}Pv&A~U46&%xaeWM^xvL1 zG%kFZ^$I#F_V!m1{~d?y?TRp0KBA0@?@dRfjv{A$+`EMXz9_jiKGM_+mUti zFrb1i)^E#PH}GQalW8lOVioQASDRr0wf84POMqS8|I_gG@%p4c{&=XV^nC)q zD-AK71}Iesg~U7Y{Za1|OO;F5{N-QHZUwAAx~D-g_Reyj<)b#Rce^~hC5nJ6se+X5!a=wZyk;Zu6CKmks%y7Ea zQDYX2aCoavsTmmL)gP2(@<+?JUA=~RJmZMwwr@=-rIq0{`EhTp`HcM7V!-sSFU5Ac zJTOYsd&K0z0hdxN9h{#b(_}aIPOi-Tim}1urbN$q9&h=-N~{ZDD4_8)y8O0AH6rPxAU;curZC%LR5Ru3b}F?L}7!#c9Kv_q)2a zxs<|JBZ`|sJ|4iW;*aU-?}D~3rS?^GDQ7erL(@7JekJa1!ZQ$_ky!(=K$vQo_{G!S zc~3@2^ZsEM_f5LQj@ZsbZ)5i33W*(2zj zeAbR;wOoQLmb_frDikc$6Liz;)9T-IzB$|I46EpNW<0K&gcJXrE`tBa_0@c%(^Dl5 z7bIF;ZHiz#I=WbKZz+2W!UMEgarW?pbWTFW|~WY*YL$a=Nk{ z(NDTy9pR&$c*1ZWTq*%hV9;u+U9W=@f;Q>*zu1?Bsn}~x=b*_*61nFM!qSxAadbJ! zWC!oJ=5g^dVdbf;E<}rG^38uyrN)K-AORNatNzoX*o&;`Cs3X&9p{K=@wIx+nA`ar z$<*2&FTF!d-~|v!)LY1A+u(VH*ZlzL0TcA4{H@33(jHJ<;V0_d86)Lwvwd}{)Ob2k zeR%UjLd^3g&dTaK;JmNjS;FsDUbEVGpUvE^@7)BhJN$++SS@2svagU3_%HqJKy0g7 z1j3OzBU%x5qlHAE`&H_dzxd1R=2f!uO|2`%{qJT=MY5*I7yYPuMw?`LRREd(gh;B1_L1D zezhm<)##+IznK6t&D_5Aj{ZL||T-Q{sh)buSZuZLZ-^x}oGZjI3eHj{RS-;E(RyyK)}bXz{st{=hw(OpoleTyE`R)%*?K=TW9*qHEA^Ho{3=q!)gtUF?=my7sH z{+x5%i3OV?Fj@J^Zx#h8qdq0JU9;PZpK8K9_A5*_{ick$=+q^LHh;&u2szOO(kb4Z z>d)()tg&}7_GkV|mh5t{-8V5&K%50HsBn@2s)ccYYWZZ>gahr??@S{ma@+xG&UghJ z`SaW#KC`=(jmv$l{xCb+-@BbbW&zIu=8@uy<_)W zE~#?^I7Y;wmyuo$;F+AdgoMPSAG>w=kt8)%KfwT*CHZXpqdWXK3wb`l^+__1-w4Wr zyUY17#pHQ?^&z9V==bylXWWZBBxJG>UZ%=px>7%{*!O?s9?uQZ`Ofapsei>tfy&^! z_O@?ppF?hvd)af!rNa?R8#`%8<7d46?s&ainZuFeIl2bycAUK5liNho7b&sl$f((Z zFg`(hpkh?=-}q8}So$+~ct3~$Kt-qJwJ?_8JGbNNLqXQ{gI_bMzhaPw9ZUe8PQJ%e zW3MF%=;G>krmn1WpA{CpQ^@ViYp#yB;)kbF4O_!X<;DFLSkEi$dK<6s7!b%s68Eds zX8X-Qo5?30P#sPWI;qv!0nj(8c0JYMW7C#-hG~+QOT7TZK1YtoBDfJ^2gq>%HdMyEGx3i@9-#Eh!ze82G$OqmRP$yyUq}k#4y;3OxOA&`SlmJ}yLKVl~%2 z(=W9KizF{Hy5Zv8W6!&N@HP8zF6IaJyPJXAR%e!^SKsyqNALPl`*}qik&k()92;f> zha)tAKs$y`CHG|ig$t8tl)y6}R1$~yYBjR*wkMC*B=NpRo>K`YM%`X@#K=wON!Jbd90e|b8_g;Eg|mZM(v|PM^JjabRj$Di@<^9RH-K1ryp{hY){NMdWl}}+jZf6omX?V^uC1opY&!9r zP@GnbDqvQE@bcr)0ngPBTFEuGZ?HEJTh1#5cvRr|8X%CEiylhBE*3{s^Ig{mc=c$^ zI;VdQ0|gQ@6j@A8t{@9j3C*NG@=+_CD1;rw{jbC8WEEcXB5P7$*u|&gft+uy+gEtG9*75`#zwQG7|(=b zGwDS~JNLa;#d=tKw&_10P!$YNG(b`@R<^KKcrD$ZI?Nm5Kj(wwCVcTkd4lO0mQLI5 zh!9>m3}B;wHLQ2W%Q=#cw26mWo9%w_-ClQ2lB28l=C zA3SZ7ClG@h!leOD19-$lbsn- zav-$|Q%q9H5U}e@&bSu!I$s1%G8ELEQ;+)+(l$jZJ%JyOiC39jCwleLAnb9eJ6bp( zcXqf%F$n^1J~uGP5{#1svI*y|C`2~F(RMBhJht*X#Abri>*An(*|cO5e#Q?Sw-~Lp z=?c(L31tS`P1X7ebEu|ub}9qM!Yad;K0Fu(BLAC2NUlw^UGvqSqSLE8Nj@CoiDvDl z2#7g%-K-~;?8!k0FO)p*9%dsYEb>nBs*6$@h{-@Ft-ye2W!-tqIHUv7wwZ1b7Hx8m z7M7XNX>(zA&h!_9_^eonNyfc@Px^Q@n7%j3c~_ag!s&Dih%s{gmWh0P$NedNrse=j ztoG?1WBNJt;*oky4b|t!LE{q;^+f#m{KfSGf{J?a254t}P!VR!lbW{gK3YWt0-%o* zZw5RmfVIZ{NjMltFt*`U?h8xG3A@zoZJYBhbuRnTU*ao!=4UG7zr46d@sy=L=F79N zXWs-2UTQ!5RLTGLc@rvIQQV&BW~RLC#D4URSE{582@mD@dgJ`Ia;Ec9x-g?Wz5AZC zxF%Q;gC@7Pkex+BLhe(a$HH_~0spQu^m+wz{(6U-Fe&kcNu!XEn#Q(A8&0^O^8Rr# zriaKUX(%6sgljiE*RegYG?86E?Uh3ie)hePpMeV?acpXFz40&j5*Rf4#Prs{Q?xGH zc+pfJ<(?WvT0J5C?5-9Q_G}BbLT}ZDM;%qO-#kh~P(yFNn^sH|E+xR{@K9q(KOcu8 z0}CVbYH~^n9_rz{vAs(|N)v<-nJc;Bv5<;E6BJ_+prsR5zi5NWj{f^UT3`&t?*3{;={oBU&^%EH%t6A_5^K2!G_!5I-YghU&jU3GgQ- zt2%jHU6=Ts^MI)}64F~uf}w9@%FH;b_RgbFc&4NmzO7FFk@^+t77_M80_u0knP|8z#8J>(R?MPPeN~{+i7>A_U3ho3fp4B)24-nWan?iCAM9^5rYHc7RoGq z27YxBjLlaGG}P@SA03*I0XF|NA$U?s9`DSV^K!mQI8MP2`q?4cQ&khaKOr$IK8iQ zLZ&(Sy3WE9IeHL2(TJrNi3EnHTVcJP9wyGmvV6wbjEvWmQ9Yp@?}Fx=&_SfLWz^L? zyss(1>*p6){%V8E6`fScNkrr3)l|FtUX3ZJHj2-{@U*e;Ckp7M;aW95DG?1c z?*mu1v>x`a5=u);vVX?#>;Z_=@k*-RyGi`qB&_3|gFyPa9|f>YxvZb1*l2R+X2U-X z!QWlGIl#q3`S_VIdP&$d9W@JSABUFv?CQ|v<)MQ5l$8(9qbP@RO(8|0($9YVF33Ag zta!S4 zt$`{80MsUnP!~Uogj|xt+L4)aspi+^%NYCvYimu?SZmRUD065vNKH>C3}|o{ab^#2 z><&2|N&7j}8%VHYc=4Dwi@gw=$4ugyY*cT3+Klj+z% zeY^Z5sare0Na`}fxrZ$>{g?p!S_1Q3u)m=)^gSWUToD7i3IG6;%O4hOef>zHF)01U z=S??xS>6LrRr>M9xW06Xl#2Kh7b2+)-V z(o<8(XImW48~RldcJ+Gf3OL^+m!uXCeczl+n&Q}|U8RYyJUQ|jTAlH|Il!}?&>a)? zO8{9>X;Ej&2De88yq6`S_!3q2!7~Tb&xJpq20i0jfBf$(z;CYIuU5%tBRsD|ULh>_ z{>BUu;DXp+8Ss`g6tUV~pMYQS=%dqH%_KDh3Nm>2z8TMq6zf%(fBc`pHc}OSqX)ds z=3!4yjL&UjU;RQ~3d^Uw?ay>2)_pk1;fjt)PYE$oDqa|nl` zqeb($@c0}ekM}W}AiM+r;C}Hs3vqaCYI4}liS3l|%+LN_<)!5(1CFj{yh?0JOWXT1 zTOW4;fX1XGhV~pOY;10RhYgV1W!e>s{qB#AOZhNctfv|O8X@xH?DS*n7r&x+)=zyi z0m$+25c%JK^e_lNR0{a79dK|y9)?T5!?*_(93LAeY@Jv&Ta{B*k~Z_C-?HBNTvUdo z;j95nj!9(l#!Jk(Nf0ZK*~_obc_~1fNwb|~(Q(X#kc3cDLy_Q(XvyNA&5=JJr5axA zt(dPq<7XvV1g#~mDP(D&eNn@RRlB=PIQM%%w@xd@K-F=-V`zNxV|Wu*v2AgDCf24i zQ*d&_!q_$vK<+1o@__C#40b+Z;Xj-j4{q%2JRlX-k$&Xr2Lu_DVBD{C5aDv3>R%3% zOZBlnA9($Qwp^O%toQZWJ}je0(F)V1@s{S7g-mJz)X;JWtMAQMjiiGv_#Ft;r!9i+ zOgtDn%_zb{kq;M&jK3gEfozd)VUSV;cq*=c|33X{v*L%{$EBN!W4Q?B_-S)vvp~VJ z{_nprX^C1b2;LDmTn&zX@*?e^`<6Lj?;!qMw?@gXZ7#i!qS&CB9K>nADWa4qNVdIT z5S`xFY496&0C%e1m$r!7@}k6nLf8R2r68sF(&f~(0&u*fq6L@!O?&h8;W=&8W|UQd zu@#TZ`?3O=tMYhKD8eH@b;SD0#^9&5lTVI~{89`z_))B-vmle#T1r)>?2mKmdO9>V z_KSKQu9mFkP);O+@wfZiS+K8#3t5j@TCI251!aoBO! zAT2Gua7_IBDDo02uvp_i1pyv5_di?ujk?EDb zDi>4uqnm>cychC1phZW|3#(&Q0l!6$w5Uf;V)tgm<~kKdoCG4fnZLaJ(g|q$`Pf30Re4V7dp->rfmhW@rRVbqmKDNwmD7uWUp;m4^O2ubV0h3Izj z$ol8X=Evynl)p%*eFC?7tUo&ILS^1`j zc&M)(iP)~XeSe!#M}$}ZAwyJy&|S^S_T+a~R+GpA8_UnU)nm3uAAD2x;) zCnbv*ba+K|cn*<5rRMnNwxq z#|13-UsArSAf>=Xk26>LvC*AlYz4toW>Y2$E1pi3Cs?u;vVcSVb9OpY?{I^vLLo%M z4Ijnb7mENeg$f!Q8$0-sPQvLk>o6q*%g!yocQ||~(0t9=)7$finlc)a1^Kxy?!$g8 zsPaj^eZbyb;yE!I2o)8L69}sgr!LY!q@tSj4FLS+@|Aeyu9?m|;6X4YC*CbT!W;C< z-W>v0@i13$b{5=cv2qONcDy8*T+qSbq_QNXXCeR~K*-hVec=IC(ff>M?mvT|WcG3|a&^(Zqyx4(nmrtN#8VO{%tw}IWP zvY_|L^ZM6%8(Z7&u4Xq64vz!Dc(fu>J`uo#pNa}n2CRf6b#pWR{4TBjf4F+5xw8?(Sx2q`SMj>)rGH-Mj8u?;qk42j-j| z&;HbN=HYOA|2LDzJvF6gmX1a|`DUw7K+N(GmBL;YEraMQtlf=xkT9prXnf zO6eTBUOTmRKoKmoEC#3+i?Far+RjPQ5^#L-n4}G=^Y-#Hl$W%w_Zx;wHwW2!dIvs_ zu2HV*ueT2MhR!g(=S4Mo;kWcd4*J`xz+bK-yar1W=?FF!F~u)OFQ?iG<~NU=#(|EvWE z3ODFu*KQwFAC0Xpt`2o>wy(11Jl`WTeDdwbCpWghv|SpUL$g@4u)1=9UP{fyU$2Xa ziMHM7ZBcDWX%Z9FSo%1US{gP~6|onZ7lBg+L)-I|omGzYnDdl{F|{eUmyHO&04Jz( z`s$2tQ$^lTrp@fVwjLdC@_f?r`sTA#KECVSwUTez$|cO%+0ULKdUdP_(Bn~@dSuSK zr;~oII6SN6$)w6=*{b-w-Xx0b8_Cy$;E$%?ZRW(zd3U7>iM;Ag7&Kd+m`7ja6Q+Iha{nr}Oky_78*YEla$;m|9zmp1OSgz9_6F1uL$UZYH*==H(wl zY28C2!q@G6-6#);R~;Bco&ul-j89Ci#FlurO)jW!bdTd&yu$C!?!LA-a%BF?g<%yamupEB0|!hWQ{l1VAL9I!B#i-U=LPO$945B-|$u>kX^Jk{0x7DYa{ z7Kw*t_DOuy;iYP<<=cyKs^7aeR-#w}sr`$L*g;m*w*zeahYN|0vRX>$5Gw~qLn@Q3 z5CZ-$7F7DSSyWjpHLorn#wSO_dyrx4Y`A3eEbg_K2=d~oh@ z606P{B!^4Q7y@z&ak#P11R@m@_$=$&oxp`JuP&;P-Pl=<9R$PG2fTDkbFAy<1dyK(u{YJqap& zX0^H|l+8NYGmTu*_=r+q{{;aMkl@6R(k7BpEMTbcl={Iou-0KQYfz#5dX6BLUVY@% zJm4hhvv2T-7F`G7mL?kE$;2aT(g^4XYa*U|2l7pUEyR9!fYf z0U$N5#jbhYoRNf)fZKo`BIdc!Z^TqloyzZ!A|`om$-Hg&Z7{@muzTFfCcw{V6S;}l}4;7W8tDq1^H7fIf zv({NAZqAgvm0Qapb-DKY__2ZO*J@w8RSDHqY=~hCm->!_fd;Qe{2Y=4v4D<8+WbSe z@~kFYI(P^b^Vr?qT^{y)>>Q%tF1zte ze5EzI*jjG1mM+Q8-Zy;bm4<+8;c9gSXUlD@6+Z7{j9{>(LW{Yoqq4Tg4tRSOi}U%Z zS4a~YM9jcY%OPbxj(RXI^IT&fjvrxncUL@Y0(a-^%nqJ~mnu20J?WznAz(QW2n)tc zfLes8&+lVoh?jQ|Lg5d>f_?-L68e|@RT zI2PuC84_5I67N3xXDlK*<+(tl#>bBlQl=l{_&n=JpD$z&Ti(y}HlBr76|LOBw>DCp z;SRxxMxNpPqW&=S>hfFfVxTqWn=DkMaz6Yr$>EZ9lO5&i1@r@zKay)V_>dc+hUR~{*;o@4e0zVXa0h_u8*2Bd^sba|z(U!J{ zxVd*H@0^UqcRMc4%EneV|0SH5 zjIxB8#N@`tC^+0P&u9?=t*C`G0Y2bJ?bAm zcen#Maelh@9D@`CBJkx0{9YqX7ZuBkh0h&Gy>A31)KMKm9hhsPLKNxoGIIYSJ6bMw zHK$&t6<15MGGenS&W@e|yp8LnzfwInWzjzXRTKJgO4}v##>>(2MsXO8zK9`ycW<>_ zH~Y4}u`uc8vmm(s;*?^PE#u{x{y%jL4=y!6WqB+gqUn1~Or!$T0PtRS{!8*7c+0Bo zk0pD2S`0&bSyZ^ps{b3PRc^Fmb0^cm$UZ-l(eiC66~kujz0^^{w|f_lB*$M%9 zsbtillcLA2$bUXKFc@`MQk;ekG@7XBsCNr8BuMy3h1X8cJ?+|kC#7|i%?>>6=9C(gQcq@%!2`f2$_H29NS^|b5a&tQ zc;fG=iPR(@k<-yYTKR5j7?}QxJl=ei0YN-SqRe+TK~pJeM(xGzyXv2$e5V57yuG*t;N6{aXtRv!Aly} zPD=Kk0d?l}OwVASu!poq(CvO`JS!sEmLNoQmQ*r+VH#VA7MJ|ololdZG6N@wt1OQY zc_ku0z%Uq*OlCsyO-l_OuDYelmu3@5Keg!@-KiDR8VTa3UM1XW z+^~KrBJ3va)XV z*LLgCsKzLD4d~lTp1}q%j`I;}4d6BHJan;_1jqy_qE(sSIy=XOR;mG1%SMUG=>DmL z(Lue%R>AxyG1puQTU8CMrtDVMr7eeqnm6HvxeRkhDVvH@7|+qpqf_t&P6x(Nvz@ACwoxP6#Sqbs}){TsZgh$8tvRf9li6ruUN%i7`Oe?yL zigm}-+diOITh0!tsq~lnlkvFmzz`CW5mK#lT^qm|{2n)Y71&jM#wKQ;1?3TdK&Tr* z&}d3BMyeQ*gD?9hrQl~`Y+}1`FK(G_rJlte(KcJ}5#W?{2k;3v@)GByv!q}eAP}L& zAfJXB?$z|wxJyL$TQMy&d>Q<0qHS;WL1D1>U`J0NN?=?{X<)vI<5MY-GNXo0i0Ygg zP^!>Ei$ksKtbKqI_A>b8vD!Z~speP_8Ilme783ZkdHLqsSaYSn2_V5KAJlf`B zqWDT(UPl)@ScqATIX}3}LGwpK)i69K^g#N^?@(FjKID((3)~=il=v5c} z+Plh{DZ8H~!KxREfYjcaz9S*g&|s088GZKJ74HBch030R!3hW{+r*yCdjBnb3LElp ztw?kuwv8s?#PsZJKay_VIEl<4lt7hU9_MvMmuf#948hNTou&;oGkhfD{I=p{* zWw|8cuE}d!%RYU#rgiToDyKuAY)AKw&cwz8OlR7+o_q&-I@)$7?WcB16QmFVpXZj0 zUR6c+kL9DWx2VqoUB~GeDXR z5%DH97;F}v+OgW}_(q|F!F~?F`b9##V`H0w%jCPuhg1F=P8X zlKO$Ilf381Y}v~*J~hEsHvMU)ZBXI{J_f88Dq*U^=Hk;R+>gyUQiGI5w3&KL@92K2 z&@0tr5<@oK5kHj3CBQZZu?WZY*RRMsw2cc3b)#4I_IBUrDsuAMWj|@l$i%a7(BxcV zWd2!+jv=R@mo#jgih5^rNj+Nc9*HNwgnEy;uKiiO;kxtq$F|-&R99W|qS9D#HE@sq-kOa>B-mRCAo0&fR{`ZXZ zhFYWmS5R93enLPX=J9v=>_$>oGu|&>4JgW0e0!l+-@o?AK69&l!*(|iHFKo4%C0cpchoV1 z?Ja_CXpzcmqqNi9W~6^7$bnRo5acV5x9Xim`n|uN0h90IQ_M$)U6t*fJ(epXI@M2a z(8mfYKa~mb2OH+U9xS4ay`bTf)~~+1xRO^(j!RAnnN8>>Ni`Q67f)TY6&Ws@W#lC>KL`2%IH>G;wS=Tinejjz%KdFPyH2`0 zD-+A3dEEulnOmkZYHA@2Tv6_mz;(rJpLB1I$NHi$G%{5DINt1jZFr#e=KjWrK8{y? zf-G--@?x*-$)B{;yaRE6w*0wEy%f{xSW!Ti)BP|%YX=_=%_zC9c#p(rC<$xk9F+-? zTeS=O73(Rib1QqF(D)avE8iQZz>{Mu?xZGB_pDhibrZwboI1Y~ z5Q@3zQg>02eV|l0^yu;h>7J38QI3B`k%eH@Di3MP)>>*QkvM6PKzg(0Mq@2ykP}+x zo82~9_gF1Wd}UCqcQPq;x>l^1jceDTZ}k3j`^H3Fq->nsOgFi6m=!Ycsv)5h^}OL` zc!xIiRLAJpGP)|4#GB8f%ICbPzj=A$4OM*=CBt32_tutClSgBDq~CjHb779iLE2%! z`V|$NSuH!K!*bkcoLal*U;eSs)$5;YnBa*Zdy5a#0cu39MA?>y(Lu^_eAR)sW93@v zRM9!lou6}BA3rlIO#7mg0%kDV=*RRRv*Bm97B~-P7PCcoZLBMD%!{|;ht)~JZ+wz4 z;NIqOl5pG8o867$=djjTYBj(6qaSm5)!Y(q`lExyPl~!Q72llLe0x{yQEDv4V-&Wj z9LFA-lfmSP%L^j>go7(BE0?pUCH4#QMnF7Q9K_EXqUI)tkG|RO7XeG!jao+O7&)C* z>~1Gtae-BAuC;dv{Q>d9tb9sOBt0X~ySGVyX>Ut+=<{1s&7*NdK!sEqZCGrdwk$om zVX~z}nJ_sP`H_h3V4~T#4s*>p1PH-ZJLl%*5w{0)hQi1ITOX8|Yln`Sfro%Gn)2!^ z;^|m1zKLVgpqG8GQECurQyjNiLHuFfS^n7J2sYc#rTk*7;m5@OAmdf4dF3S zegL_A_6R+dxKjV3tc~Av?*a!IYi=K(SFo^L_sdsH6F9v!H8Fc9$tO;e_B4BjKN>lz zGUp3VW})2>BC~kw<81liTa3@tzwy@JClmlKi$>wa!W(z3eQyA0}{imx7lb}POHh5SSM@yuBQP;6SbS4ccJ54!j`ki9G zJIM_TqtpY{duy1xZG$64KANxlMD$1wMrp(_VXyT(QgGfvWv8cS{??YpiNV)9)&8x{ z#&e!=X5jm}+#?_v&-*-9TI(0M@HB0Vm69gUw)xRFT=iVE40HyHOU@UU?E|qh45YxN zQz}*uVB){W23AoneMu)rsvCloloZTH9F3F%QYhQq)r)QKn|?x^C^&Bfm}l?f?YuRo`!5hL0~v{60{&ll%|Fb_Z2)iWk?ilxLniskKHG#)T{Iya~VLz zfbmxmIb~Vs;=NNvH#jzhQ&QqW?CLQOFfLG1i9CVhZ-#i0;jWFbzJX)9naTCYMs){Tr`&iv~>(hc!1m1BRYM|U=O?e$2qxNqOekYvn>_~~8 zE^-!|3Y&P!A^_qh%~g?rD`7B3TXlWV<#Z9E9N#|So7OL;hIywW{-!p@QH#@Qti0@(T zBxFF#M>#_U8biq6y3a7NF^n#37!|QJG!m956ey#4nFy+?qd0c_DPZpx1E>|br zx0X|a5+XOBK2|VyOmVk#ckbZCe}o4Z2rOvYp7-A)E2S-@RVtj(x{*SZIj4a9sd6?! zIGJuLW0T4(rUpof+vS7Nz}>3qHh(7nUURyT`H|ORVV|QxuPpHM3YY3rRE;HpkPFTa z7{&%AEdPChj)g%e$C4+`UbCjZj-EeDnBteRufXa1-)97jA7=dgQ-G7SK!k5Y#ARq4 zl&`2duFuJL~&U{1#BuALZOKN^Ax?&^&95FC%tw~+BaWU|KBZG@8jjRMR z`}JxL6SG0}?$ccDea1$qeW^i+VvKTnd83>#7LZoSh)G3ez915h-WJ$`Ho7{yB(4w`F>QzHiIY)NO; zPvq_7ut>mezA5+t;C7Jkxx1_IN^JB31p*8&NHfTG6#N82U6MvJAda1s9T2hiK7L)< z8oZim83hOeqbOulWX~agb4sA%>+Hh7;eR~10|ibwUy(Nh;13_~9?(Ty5JmWdzuJ6Zr4xVuw;%^qf=J^0 z@(4{tStKiLaJbhhTvu9`JZ(Te}?@lJ*D z$;oz5%|T3L&;<=3z;MPx8r!%M7ng7tcfAx0P#aa#WqH&nf3{IqSGeNke1=)89}xI! zHx4KVZprMjsF3$zMM839uAkFZEILkhGG!ETvB)&l0rk}KJh2Bv(CWkr<{Bir1vi37 z6(~SGPe4w|`k(aHIKOQScpvAR^IgQC->@K=0blD#E*r8~{R>_O&A6TTR)yhbkJU{l zO3S(grbLkN)J)=61fpWIjK47q}L)ntw*7t$lQC^^S-r= zKNJq~GUTOdlQ|4We@USd9|u96oqs;?aB_kM_|7NsAH<%IRAQuw{=OP2hKA1FCCt}E zDf?)#P&{X&i>VWn9%5o{4x8#b_TP{YEP0U@8<}30wVGv>}^=1 z!uOW_Pj7LNWo4Cq0=p1wD3B)@a845r`VHIvK9$e@r>q&PVvzhTu1{dk&dWg9Y87}T zeC~E~%^vr(MZ|fdfxdM;%G51?5^3AFK7}G2@L?31P|FBhcC`6ze@_`Lu28Z(k=6` z`=InuX~3Me(RFW3k9-5p#5HWcqBZ8nS;G+iQDmuy!Yb=j^(LI@`pps zcB~g!RLv`KzzvC;C3V zKcB{3g`od1(c$3e0=I8&S0)+9kLMoaISHLNUki<~Q)Om?lMMocWEYVBLHsP3Yb}pPSujL`JN#&7M zM*iHZTCJFenP*p|}p$M9FfFXX}t5T15+ePWB>(~-uNlVDBh)zA!S9M%-q z4NT(YrVb@UI<)^or`gTY-4R_)W4UGksuq`Db)fKwS`3Fd0!N35p!-Nb2k1 z;QqHeZOMe2ThmrPc;SjSGAzYAyP0GkiYDT;tt?6_`uza^4SsMmg>pWvIP6^j-IMs1 z9s~aJ|6_AT-KooBR_GssM$gNuh&ysNHC&C6B)K0UOWiQ7s6PH;*P4L}cS`O{8G52l zzR@ohiw@=?cgk+p;9o)UYiztECj7JAys-4}<|AdQ7;C_+nb_2EcPYo;tj8ao$Z!8Q zMLezwazNM2?1-4U{QF*c>|@*>|5+Vr1}I`UBU>kvy%;OEQl7PL#9kaSZeHM9sm^!L zN1MMkg!xtbeKaeqFD9&!w=AF~|NmXTTzqQSFP~@eCl(R87l|W>P)Vl2_QO(dr=4YN z6}LlQIQ|gmtc3Q8nmFz5vx@@~3=>{W2F+kAA~H6TH~59WjQ<-BWHJWf|6Cp7z(!Lw zTYUUDPR^d02D6izgAghNEsL=?A9)L{jUT1yiLX)K1RtoKt3;^yAk9Qf+Z@mYKiK0* z^4WKFF4!I~gb@E82#Pil`OKBl%lF5io7|TCB=a-?C4FaFZdD%BnhD;3NtcY|MHT2>WRJz6 zPObKl9=2<@c9G+9J7s%Lx)rlERCPV|^_VSIJ>B2dpB`E9(9|;9d$?jMGd~n+*O35< zHhps?ZP-{@iwk0X7?4ui@(uux3+RMiZ)9x%<38r~(nE)ZkdFuiqU2tWlG3bM6YFS{ zvCTdP(s^Nt9zw88%ghCu29JgUCNXT)agqNom8uh!nUnE>?GJCyN~`-gZFFY_3gIpx zXPFiMz00NP$l4pJeQVbZ;g|M=J%m@KR$YN)5>_rJa8W)vn_zdF~H5j|8IjKk(VS? z=Y(MZZ%_dE=pD$b$IbcOxK3R1Qwm1h;hJeb%0`fd)+s1{iZ&$W9jPu$Ge zN6cPA{wjU*f0e`6UFpu0QCeTSu=cI$g%-}=Zk;EblH7V^j0)0x+Vs#3Amsr7>s6Qa zDSIFxeb*`~FetDiU%24-n6b$rxwrn+9*iS`pcU=B(e8d8$oYN-+2El^_iEyd|5R9! zuSFQ(M(qfCIV^gD?Q^P;MqRcEbRMQBRM_vO?j5cmLtW1^e3bpLE#8UO85~z!biHZW ziCg0!zU;0p;_~}h&6n*G230sZfq32b!TV4yv_`M1!xr{U^z*?K30iD&`ZKt8R?_AE z@FShSMPXQsKM9UlP}UYis_C%i<#2ev3Q7o~UOcrR&vsp7URx0PJ=l77e@J~X{BTX| zo7emfkrQ3O=_{uh4<{qR1MzJEZGI>jHw6*R6VD4sT!TL2hpK7C4Oo()!U-MH)1=?j z^a>v@qd7ERpF45BaGv^VWOAe%7C${Cbyx#B9RZIG^N)S=Su!qI5wg?&KlPp`AWUQf z=kk}x8+P{EkZ{eeX6})x(T+Qc*tLg<{?xlN7eervXHRu1)1Hiwx0hl(#gt25L`Tu) zC7Tc|cuQt%6{oDN-#wx*c4_Nul5Fu|ug!~mPlAe06R7f^U87ZAu#B6$EzY@z2EVeF zIBtJbJ_cykjkIznp0t7@pW$5>B!aEIO?rG$ti`);R-9mmAdi-Hg4<)6^O+kQ5Jgx_ z+iBwufQcfR9uvN0zCT{Zd3N<0cN2g0x^2zL^1(G9Jpgw2(ET@|2YzD%WftBPTQmJc zd(`VyyroxH{>5dx1+(?&UOzvnD&#R+zF*RUII~D`wRLF~OF_2ZRR?0`K)cyO{&cZKen> zWpASXsdSeytnG2WDk<7la4zS+C`14f))ydkLRzo!yS+0e?X?_7Kjjq^=-s16z9&4? zXz*TbKb*?f)NmG-bjsl_$!iH+pP7^%S+%p|B9j~>T8bM_J zy>0L&NU%ZwG}81)nrmp3F}Gj>RTu*K=VR8|vH@*~ip#rqHtE0UVDz$Dp6)7~ZgQcDtMwn&?8~^@YRoBtk zWUs0MY#Z7S`m{vU)hBfFf@>0q?%mz7u-@P3Ztd&_VtFIxaneon1Oz`T32thyug|8| z@J9oEDVgth?Sh%d9O(>irJqCeV+`2Lv5TpfzOd9(&e)##y7^@LlYQWQnX3JW_m#_! z;DGDp8qe47jZj)AwOV#pMl(T)tT|syvO!KXDlV#Hy31cV88m)jM2q`3++3jwr=nWv zADbwN1AGIHE2AFLa^DxMOfK&Vi?Nr5^!}K8>i3{`cYFszRvG8lz{Lpda7E8(tkf`^+i8*R z0FJuEqUJBJHn>4|aaNElI6k}~T4d9-X2TA(>tE`@MU9f6taTgb`Ll%dzEi@Ufl zpv#ZDl1A|0O?}PA&e3&<(_pa43K~jsN@#!!j|sx)OpI<=pmL$C(STVNo%#wunrMTy zgH4{GSIXjUQchIj99lo_*A7=0mOS06qQ(dn}7oL186m=ZVxYR(GIU9LuaB;p-juN z*A$@7@v5%*Cm)CbpUv-??v>i^NgMAO>jC-NX6yRFvt?~HAZWlH^x*)71`bj_TJaVy z049zxnALnAnG0e*=rS=6^LfE* zg+Uks0s*-WGa|FNxOgw+K|u=##_J0=5E@&XvtQC(sM-6cyvsJF=JD`=iT}!H~9qx)*uuFn}f@O3Kl+N9w5`mb`*Uc@1WhEtwx=v zV+3x{*7$88>^&iB9W5#$7-@pXd}w17SnCh3lF7D3QffXL3yo=Kgp z3;G^_WDgH{b9)g!FLV%%svJjIgX z?`D>Bo9m9pJKrH*AojyIdyXZ4LJwS@ElbbU`4L;p{{@Pju1@^CQ5(9%j^n>?))5iB z{=Px;9*ddNYr>xsbYa2zQ$Pn0q;;lcxR26QaLAwe=#`P9k=u2be<2>uD}z_CpfGTd zl}~3M-DAYY6A}_vJ40|FUdl8rfwKu}U^v#Sj#sEtJ(-@mV~{~Y=|Af1W2aXc2Wy*+ z?yGmiu%DPHWMR2UlA&K|5tL=Q z1tmdk6o`6*%4TsS*nq!gKDUE(${r-eDYlvGzfG-6Zs8q8}ab*V0~2= zoFuXS2o31WS4*mQ=$rH@9h4?;0wT&tXE6gEu0)FnZ*O=G?@UN-`DWXp!$_%~PoD}H ze1Vw`vp<0O0HF++L*T~f;kj%tLmfD9oN5XJ-QWH6cv~R!CG*h}-ki7>16OrN)hY7I zr%FaJOckPXdngyr*UBsD@_oWjv9zp4=aOlF>Br!hfwi4O6cP9!B5>-F{wEX4=23n! z$4K%=_x;Los69Uszn7>Rx@5QSY{Jbt$^Ye)=Bm_oQfc5k8AWd)MzLxV5PCox3mYn5 zBX}lwp`JWd(`C<(Y7E>u;C&>X^Ktp?Ua+}MyH6ABKYhadk`3m z_elcB^`Bg`0uUkqCicC!90)0^sBy%w#^SNJ7}=_W-ns9Oqc=7+s>GOs!#cPn5CA znT;+dbf&eDcrW07!l9^BKw3-7I+i!~sN!jkUH~V!n%%DPwz~iYpEeN)gNAUW+x{bk zeAPfP1ZhzJy68_JNDVX_qC~=u#k><}n%~x(09RVEWiFE=I;e;APG)d1Ab`GCQ>(s{n^WVjaCSvHSE^qqH_}Uwfv)jZtkJEl?xfo_2+N>uI@nX) ziTiCMjSL*LQM_WBo%pN$UGIxReU*@z)2&N;My!APf!|j@0#+Afh}f$VJ0>;Y*-xL3 zQQy0qEB|=XbNvGahop|h)I_FfUQc$auZhe{gXhY_V_Aw^7}_ zt7%N(df^SwfWMg1n*`Pj@{U`s2uw(i?hXA7X{nB3&(_m_cIRG(IuT(nALE zegj{sGk>={d1l3Sfez@3km#sXCtrZ41=M0Yk?7BBZ|Ou z^8mv1u;4o_KMe;CNo@}&B@HFp+;eyb?QpQO*FXm&2-nfN$^9NQ`zW~djnp0KT=u4QWkYhWlXBlQa^TCKbE?R5Vbfvb|GPE>zw}aY-r`!@ zid90&-+01EZsm*;-RXKA9#fw9uMP*r`gB+!NQ*DU?gQ5>n zvg0y}(H}mjDSHDa&P1NIt~$GVCpn7wBa4Misvk)eISGjf zpT*Fa86H|u_f&CpwfWW`7&A=9SA>}hE!RZm^!Xxxu%F|kYylsNU=~eF&&jW zy>PZh;*(@{f1jwH(2)^~1dFqR_<;z%2*GQKMWn>39~P%7)#%lL-H^eKkHT~MdDJF< z5R%QL;V2$Z2HqoRgKYOto+F=|<&6s*wG1RH`qhlS4Y z^Ho{Jji}sc5fFpt4R-z8Soj9G$kDH&fxK^4vxbk4%TXsIrlXfWIX)j%mki=HlqM7G z<0rTIQ!EyLQN%mmhd43H@ZQvkAZ8c%7O zr7k1N&kru%$BqfsML${$+=3yE=HSXgdSCc>y^2QXUI&}#>E!WoF5Qz)jCy#_1juP{Q6~j(KAh{{&=tOMv ztMNN|EJsquO$Qsd@{z z35$wJWHku20J1vZzJrGXl4KxXumbQ_t%KO3QG0m~)vry&l??+XHa&CqL$u7hd-11Q z0dPa%hra)%c+Z%*)I1*pUtL=dlh8Xqx$ocXF)8|I%8{25yq_w8`W2~{>Fg;rmfKKW z%M=MLC@QXRr2QX#)cKfkKKJndW&yrWJQLz#05)J4lN%7b?{C+qkZ-AxXS#-|#l}cr z<-5)G*TD5f=N@SGe*oy5`5`Flk6rT5b9njt+w21Vx%8!FkAZ_f{?fmo8}DX_rU4m* zJ>cTUQYA6Csj8jnynwvljih;2t^np!c8X(HbyIgA?{D<}=odG;UzzG16x*=|DPa|N zvn-sdc~i#b$91ze^Y)m~N6u`!l+PVRbH1WiL*{>Tm`uXRmr)ZI?rLhk$te!VuKlNo zBJXtn<+__8z1wAbS6G=owN*b+NQBR-TfO-g_Ch_)9KfW5*@2A@h%L}eg!%({#jt8Syt=JJrx?!xXV ztce@#_bty9zX#e~_S?5~$RSw>BFh{{;|>sR_)WO(vl`vXtD_N&)KxD*2j16OB(b)Mh@2gAJv2HCDM ze4?I-(JX0cX{h|*l$}d{#JiRcX$)n3SltQzR1EgaZ3mm@H}~W{&kTOIGrBYLG#~I?<1eyM6grt(I zYWja{{n`%uF(1XS^ji$^6^!%RlMhPH`?f)Ai9`? zdKA=XyQ}Xh7g}Czot|BdJ%lYDU3;iCxD}B+E_SHZ>1Ux6^JK0mLaU6XNO<|5oSUAe zu86}L4v=czaZ+CYN^N=DA`muz_FKuLP7a)Nbab?%qrxocT6QE+K=O@C(u>17WX2( zBFD$mP%W2JDubgQB?IBCP*UzxWYXmd&%LbxDs)2PdgK#Wjz| zF^l&PiA5fV4K)W2Zf>_v1rSK-1}tNo%yURQ3uP?Qik@bB)NQ2whg$vwh(_$HQlXD>$$Tw`{)*P8Wc zWuZiDCc$e5`!jPD>}R9#emGT9Do$~?(95YbZyHCQl0nWJ3!fKb)W-LfmrqX?=RPC# zPZ4()(4|LNsSqjHKn2M(PVYW!EwISMYPk14=RayCLr_$e&Fc{MzgV@@;MYF=tN(D{ zo7z7&H+MQ+u&J5l+4zJZHX11`QK>(IeS>seP*WfqdiD$TLzk-_J*hA^YP-*@fk$NHGO!WuScIF@|1l=o8*QLPvaRLOWV>7hYJ zo^QGB2)>b-W|no6Wo9>a;DI_eZju>z5XkUX@O4?~W^by0adcGyH6&EYNh2vuXxsF$ z{)BhI9ZpIuae=zUkpHIYy0>ITrp#TB-F?1YVVo6*NS(>gB z@uL%y_Xvco@=vC)3^lj}155&LmrF=#ZOyagGtQz`^!)a^=Pbut(uwU1MBv@FQjUxT?jcQAv#y~Fl{w8y%^uC-L`m6Z z7Ij}Kxi+b z7No{tj#SczUIA=7MW({*j4ZLeqk|ki-i&dY%$nQt?IJ$1rimJn@u{k(aYu{65eZM1 zj-4{LuS5S3rudh;hg*~ETU^u>`&lQhSo3OA&#JOMNi>*eC`B?hUCK3`7V!6$iL4ApkhU|?VX3o|mc zC6%0<;>{Fk!qd0JsrsA4MEkP^j?^=cvKM78#*c#eB?ZbB4#lA{q|r@X#e5HUxt4a? zu?=6C6hwzHc~3hjOmi*2ffw|JMHN^+H3!NB#+IZWsgjzVm78oDu(6vDeTc@5%>}g% zJX#`9B3Rz;N99;8!V|=^sb;;YqL>3*amX7EIZz4d7#J0IR^)*AlBX=hL`6wMn`zYZ zi9a>*uvv^_!Tk?V*%j>z#Uo@RLIt~}YYZOnpl{|RO6ty&_TV92has?mHcucTv}0x) zlau)?8+3NOSmZf66!|76`-jSR<3y6gqd9ACtcG9Zg@5!!B2NoLW;g2k?T4dWgNN=9 zL}RsV^YEH$zOh5i&Nuj5>}GNg1{xb1adGh1Cb!Npj;EHfyccehJRUv7NmD!yx$Bn? z){3^_MgmDN-4D5IJ`b0s?7p6q@AZysIU&?p(Lq7AbT@ibPIcdKDh}FG!?;6$KvH}= zIw24X`BH-*!oA%jQ@FIW-Pp%Lt*8#RtShmxkd94LwN;_43uz16KX`Hi`n8nr__R+g z9GaD6<SHbpLbv^+~z&$uQP3dVsAA$^H7&+pgMajm8f~d5Ji6Q?EEQ(w4)&vvaWT zRfO@g5hIZto;^=3sLNC3{o=}e34uUF(?JT%3co?~ZmOsv3Wr+y1193J2mQOL(U9=u<09d{(fM!n#BYesHdqy$4I zw@Ns|2i&u@SvB>jLuA3Wg%!Io>*X{)r!$0!sR^_F^)gs-+~I^k{{Hz3yW}8uvq=?G zgs-7dmI7WsKX`44H=odn(HM<)Z>g6s%~cFE{d`$_mFD_tluby2UppVT%460%a{3CH zu)@Q}n4EU5lfs_U2$rt|zj}Un3R(SD&Q)9}m`N@y(lIzNp=}dxvz=mpUQ)vGs9*oZ z&02D=aA>+r!M+l6l}{=u8P+aI@FRSF_dM~%HX>LQE^DNgaiJ?C*mBtCtr-|`p*lk6 zUdByN6L;BqV9um}gH`;ugZMq2T-Q&8A>l-+6CCQe7tfq(bwCyW*U33S%wKFDZMmb; zVGXV7-uznBr18I|TQTd{LAlw+0(RSf z!ZBtNSM==R$gmgxfUOVWA#nXZ0XS%sa8}u>c@^|kV3p-JX3q-{g4eX#j^sXH{#gBS zlE-hb%jhV7vTH(7G0$mBS|a(TlDWO>DsHgC6Wio_>d!#*5QqGk0|$fsLFTCce!nF6 z{k4OwQBetFmPprk1)eP)*{cK`oD73<5}{#^p5f>*Egnt#3=Gm$rH>C2?>|m{BX=rq z%gZZI9w`iw2xW`PK8uUs>X5rL9acWX01F1<#0)+hc78Lv&bA*O0iIG!OG_)kaY7O0 z_=+Q`FX?k{FX1Qs-)jzXZjITYq?XLGP%HM-k3SbGne_$iDzk%$wrYuS@H5`wOR>b| z+FG$cez)AwdZmO%C<-PN;zSLbQKwL!>FHbFl9poo#EEwhU?5kXY`5ZzVvc`E~m*}Y=S*Y$)YSq9*4%p~ zu?Vrg_x-l{OoOWVbKOgZw6DOMEvv%d-g{4eeyJR>E&cB{Io>HGEJWES9X6?>G)r%m;eY`)4VW2C0(Sr!nUOyJC>_V{4m)z-_WybK z-LOxxzs*z_8W<%XsYi?Tf8PkgCD|PG)qLLM>@%DlNd#F0;pkNTocm5n7UC#)4J5@M zw(JDoFwlNev@Vk$?4MIF%Mc+Ff42NXEg=E7;wCODrHn(6;Svu4R2F~|z5tZ)k4z(k zcbmhh^!}GOS|*$oqciGb*0I6!Q?3jeb?fvm>ZmmdFP?)}J7I!Z^FnW%M)(_4R@2j> z75J`FOZOh^9>~8FFbt4cO+oXbz#7Y%@ik}|M>wK@x)3`5r__({hF#1R7wOnPaMXVK zfA=!0xw1KEYGG=VfAKp|?mu(_8OoY!)%V2^ANgOsm<{m5QETyRivJGY!^N%F;QpB*J3B{W zDEOQ+V#@Am1Uu?@Rn-71SGu0hFU6UMg?h_UkjN7Y+LMfH8};}=meKwbu*AYjn~N{56Z zBHdj|cgWBTAu1{YN=oMd1I!>X!~g>ZQqnVYN)H`F$M1~q@8^%7Yq6GVn9Du)p0oFU zo@eiU&i1Sg^KQSE?$TJ{Zz7|xUb=>*XGD^NBY}t4uw@giWuRn{+|M(PGZtSB%*qz8 zR`X1DA_{ZLBSlS7?=wS(aAT29%f7Ce++M3CMGW!rIpxM^VXRprTB_C8JA~V-+Khk3 z*n{Bz>%Z*9<-LPE?(vf0S15g@Xe}4q(7c~QnFnUBPF<&B6l-Rnl%+M>u8W_4IosFP z(wnwJUl1_NH)!o`#?SSbYdoni|KZQ!S~KG=vbRYOSeO5WWJlfOHK{eIGz+Rbb$CRR zD`2}A$s7qjW#;46{XK-i+O=K_Me?N-mQWpq(ZocB9eY2UE43Y$R*2Dy=5FPVEfJIR znC*D@wZP-XUW~tve~!ue9uMZ=Q$N>@>KkO&H_x8ru((z1JFefJ`IE0WVYRt?l4J}U zR0xW$9IrmfH`O=G}N$x_o76nuU>cSATdTV_a}HCKj%f&$`x|g%d#<5%ooVJws3HE&A=sKbyEazoljt< zl0ap}svr1eYY>iYd?LdG-EZLW#C&YeQnb$$LNrqIrF#id?Dw*yTZ7hcop*7_z2(IQzr_%_KY8-D zlf(l)Tm8t8d#MTS!^Sb``-qj}y|Q@6L@EK@;f7t{|5O|bP}+lisjWx5g3f+KI^Lt> zpZi-q#^uu$CwZ^gc!p}#TK3yfBKGP?zWZ95zp)}H@3nCrBj?v?TE=>rS#f<4%;;-; zrZd|lVk-~KZZJs%rN2gK)k}`g!?Q}|{+O}plPq_NIOvzuBg~#q(efIPI%tz(-%Sy? zcV8jCkUr|6T``KFSp%bQyd?n`* z+P7j}XlP$X{f-g@Dmp$>#Gezmq2~-;ydwr-ZL~8WNKE{%uxonP15JQst&FJKm2tiL zH6daheiOjAnOY~JRB6N_jdFsMNQ;XrNt(ID4F9#FPFUJv;MQQRQ)y*6F3m|qK7!G~ zrY@6mv_36I+HzQAgU_NF{JOSd@R93c{_AZs{s6ZEr?Iip^r`!an>P?$^h^0?pz`EU zCQ+l~m(<}OH0@yJnNJ}cS8{uN_*gW;4=?Ves;Jv_GPYiPm)B^XodGe zqp67Yyr9*?AGDbzxoLgefyezYiJ9X#BaJiX*!`~K? zk*{%}>{FkbV$s8}aqSm6R>OVDY8v~amCYPg7?Pj;E~PhjzH96kk{|mo?xUJ<7fOC+5B#rk5ZH(Ap56{ycwXFV%k=B%keMbob6U zW6kZ1X-`TAg$|1O^I^Rr%rZXDaoXj)d&uF9eOS4dVlU9B9H0#WTkIn6>iL- z6e%VX=X4=*frmeR(@~BlXpN6%Z>&<+qY95TJHc&i2-l2((W19%T(nf(IZRi_hSkve zIu_ORS4Q>C0>N28%Jm2I2>9GPwc4?E#@%#M$0L>dVp#QL8_{%rawzh8Svl-^3$O%R z@&nMUh#Fk}ewn+N%Nx&_%!IKyVQg1Rc7AXJf=uxNvIW1Sc zRNB43cB!2l!ep5Sjg5559YR0qqxHKBXyQk6v!kRlVtqY2t0P_AKC{bg@+|u;n}UJy zh+&JL#7xm8UCaBH<3oBM{-u4mCR>^Ya*-uRZaxsX-`^0|8k^hOW`|PSwx$>2pDpLU zRxLmm>%9+ppQ!_2BuxZC*yyO)73Q;?tG8OVvzlIk1iS0k^+XHpEtq5UA3>?qVIL@< zA$fo;cr9sz(XJZxk3UkoOY3AKs}&u$Q4awS3`SedIj*K*!1<*(sokF(!7=2B&u5@2 z|2A@Z#*ldP-dPx2WKf8=MM9GQuINuP?!?CD60h3UvHW)#tw}pRmn#HGNRnUFMXwiA z&stUjF4#P^$a-?;uCedwV6>IdM~r3-;zfADVj+MbPzI=I@rbSsNL2_KtgWLH_WSzm zaLpe(T{q5S@jNLL21ziTr2-@PTDVtEh6GU|n)bxOc@VzNDxi=E#llU!#po?%YIaH} z!(&m@z9{$6!PiUH;2b*A4H>N?_Q)@;&%aGtK@xZW4XLLGpYvL^ui9A1A!H?cVfTFF ztdL#Pe{=gAb#1_HL64qzKK`;DxJIkH%&#G9#UJ6fE4_D?3bRnst=+`qN8G)nb}mDb zJiS%7@t%DCkEE(D8hcLGfAjnW9gC1kFKTs#eEwGf#_DL1EV^14V^bj?d>Qts$kdG z0901q_?oh`b-W7&1(8;V`*m(k^=NDJVsdtUcBrZ$T{boezqJhQcAGLTuO$Py55Z+j zvC$UDMvlfBndc06+of}e^m%8bEB`3M(zA*B-YF=<63@)lzfSp<>?1D{K8>HG*9IiA zpU;5*qbj22aMG8CU~mLxLVGS#F;*$h#XoBcuM=_o(SW<;LlM1Rc9jQT=MS0PN|Hsm zg>^$RGXVsiqi499-I8~7`?jBKdG4PyiwBwv7G?H5MTPjODvxS;UZd0|5F=%GCRtf# zUwzVdtCwa3paM3NB}x#y-6tymjWP!vQ%s1QFDW6V*KQ~2IS*vyREe?u+GuOP7$t*d z78jS$GD9z1ytq<1ld-E>a7|QbWPp1n<9grkkrfGc!}Xo+a(qAS~bUwbRh`v`FlPH}!(W*OhZ- zLXV8S#@_?n=5E=v(RP1=A~#Q^60f&p`tsI!2(tCI+haU(9NQsv;~ls5XvTFHa-DPU zQpgsP{RZv}C7KZf%=Xcb@@f0{p^7Z$0x`{(w`f3pyMo6?`BqvOW`pAdWm&x!J&I1s zx`OiEI^Ww3W$2RcI=4LkJ+wd~U;sHgvy2+(&Zc2IHKEp07emL)xU5q)ylfO#*-9R^ znuEPV&%3?Oh4FEmI)Vu`$Ayn$)dc~YA?h@=NghZ?G8uTDrHwat`xYVusQ@y0%hL5- z$UO)Oa9+F^)Q5X(-aKaE4OH~<^Bc6w3$r>YMlb0e>%Z+PHM(%2pfXab>MO}J(hP3L=)8|-fW zb9M&t`Dv-K==Sr@Sisl^U9833z>K=IS*&iL+3O z=rdNxE%{7(5Q}dCh*cLE`1MTTb>E-bSTBz(*!3v&O!jD57nwRf=&ojNY13AHyZQL> z7!Jqy;8jlM(}umlnxOh@a2?1JwHlxXNJr0*{L#_`U=~D$|Ek#zl{hWht9P4K+C=*JnCzB12|K18=$kuK3(s;n z)GP5ycakxCz3M>KfnXfGs;aHVYt?!(hcZK(D;)72SD=iVms`Lkw{P6=hbE}kFRv4` z)3`O`w2#`_)9wE*R|+!y2dzciZ0{iKH>K&$jKQOGUKi^Xz&5>9eYgAH#ZbsLy;J~7 zfGRnG0ze6ZMLnB-%}4NTM=8G4pIKYcCLS#IU~~}zsVHf7lpH^@eR5NENc&Rp_&tA* zvP`T0BlOd_2elQ98UV@ioyNDYy+aa52!-{L&g8F7A-M0?F#@*0y3F%0@ql z`71Qn&OiX+MHC2;J7jrNEbnKHcOklUFXPsDgRDF)lMUzP;0~K-L=|OJj&0~AeP)r* zXQ3`S`3U#p(H>xP0Dktctsa^Ixc*LyBKUwj;NR6W{%bCXKgm46(1=kPDo{d|6LccbM2d;919|>bdSmh& zgZ2XRjGno?n~g}a1$s^f&-rt-z4HpJUEt>d^BL&V3ou_3MGmcPp7%uiwdQU;glv=I z1=_WSjj{Sb2O=*O0I2B6Sfxzjn|n5b{>#_=&&|)l48Uy2Im1sX<@g&x;7d>n6Ih-N zh7ZAe+xwdKXjCg3yqEv?v1}n(S$FGYS5AG#8{DU&ULxY)5j9QJn7K`!OLpc?e*BR6 zVX<`GF1s`QSiL~T?+@%A=;$Q-Fe6WJp>SWH1mTq5md5xNOzr~&^Xks*R$57QLfTH- zTn7T&1BBb41MhLik;Gb~03lwN^T3tWelC6dA@fB@LYO>$y&05|HIhvwg)^DqBmlA- zEw@_CwBXqm0NbZO3aLZL8}iqob!Ffts=^&XYy2iQN26DuHDNP*O1I?loSdt9ZLb3lIEl4E~cX#*-v>-lrEA-!pA`5cz+#04+9U z3;S*S`Y&XICv|42)l>uA83?hKf9rH+N5gt z>vdJJ<&#NVH(Yq;j+szJ<$RYN;;CWsU)I{4tb&b;WE@oWGi^sk7cg?ZVv@>x!o=!0 zarP;hr=M5d+8anz@&oFh)vjO$mdN)gKT@ENCf+G(14Q& z4U^FGGIsS99aSR`zR1!%QSp`S@k+yL`#GZb(bnYKbCvQ;4@w;y6QKY5Lq7_{9?hh`y9a0NC5x!lvAyiR6FDiC6bT z^>xE&B?(SuW=bg!z60Q4s+-ONkL}}V*BVd1MTlc1Ok$F$bI8iK1P8DHh~vQIlhk__ z2_29(0`oObG-3iLNRbqHMpMp$FU{AItu1A``nvyTA>UE}MzEpwExlGT?MSctIt3rM z^?k6OmB7Dyl!^jn6M{xr$QhWQ-9Mh_-V~cKDIff*esO{+oOl@#0;e(}(s--r*uAPf zprn90l#gW+=F1jWdVYkv_QcPbAzogDn913ShVO)dP-!Ic!iP`5L}a}u9S*S2K1lOEdYGixEt2*lSf!&nDi;$Vf$D{T%u?>&=2kIq;bSYBun&5p~sui>&_7 zYRvpQUoEdCsQ`t#LGXW8;51q-Xm`BXS(vpkJ^9f4Pja5*Y`Y-H6wue0(zd4;w+~k; zM_cTZdYSlZZ?U?%-t~=4h%bP3vM8sPP|dpp?rrrNVP1ybLwcu=?T?NJa+u4Q|3qRd zn?FCNA&Ne)`RFZ9NLV#Q|N8I&gaF9I9$na9kS>k>i$OD$uIqQWd)$H6ZUQ}MLgb?K z$)gP&AWeef_yob8i%cq3dVL+KWI+?40LMnG=kJXr5BK*5K+pcMzXL?@`Ro2WtKLHP zYzxm@F7uk#j+oq{6}W5t4p9-X`l@mc_+`@$$%{tLJ?y65)qYVo{{r)$5C;puE&1@r zkKkTc*5dz@1ZQUEj2&uBmh3zkezP=Ot=h-64JJ+<-(s;d+7KFa>X@Ph;Z4s=u>$B+ zkp4FA1lhI^97;h+{o$A7UV#2GK_Jj@wnlvP{vQst~uZ`YC{IF^ya|dFKwg_wsozvqQOnD*@;L&UJOve+B#XGVDZp; z|L`ALY8|wzmxs-@(}PSD`%PWM=pdi7m#$JmoSlsUMqa(A>|C|4o9NizZJo zS3iLNeBQ&Spb>tH`CF#@$E>=5$WGuidu~`d0&@#v#y}I3jNrtZD(i4W+VgCBS;@uA zU2@r$3dTe-curLZOisw2qM;qUx8Sg0kdFfSjueN-laoYIyU*m2mL+oaty3wN(JyA- z=wC?JAI~JdzC$)<=pPXP7gajMA>zgCuSjx2u=gK6T%B2+TGc5exo8D;I_NT7TPUmv z%F0bLl3O5O<3YX=m17Z#5E8+}>p9zdYMwI$7sALO)1fu)y)nW_j(jKP=GEflEYSam1(B_Yqc)j9Qn>Cf?vKKf( zv?Ti^+W4c@AY%tb?Yve{S^~6{Gz*hMu`_#T#0iXW#A^x;8R?{$ybWgYv(Xf?J36?# z#_8rkl>pXdNLSB4n}LJFI=(;<-@iYlt7}puH(Y5<4CTWOkpI-L&SCYKs~pC}gR8h> zv7PK7?8D0t%-nj|%YKiRg#b7@IO;Iz>d^6lC}E`Qg&63UBIg7f=ycXHmkGeRCkH5kQ&2-o@ZYClU3j}EVm=0ySh9>K<8 z>;QBG=Hpr#45rn%x`!-NK6-4V6^)=z?ph!Bc5^n^dLVyVL5z^J+&(nv*$g7bJd5A2 z#%58mW+Z)lQhs8PpAp66OR`hycuQ6UKrM`8Ob<9_FHfhgm{>@QjJ{}3S2g6~Md6hHeH9n6^k{I6K_OA*9 zl|^LPaKBWl4S>wxm7k*b4DJRV!q1Qvc#{)UOFVKN`wBZBB+x?=;{QvmPVesiORR#e zG`Jt~sVTa<=ia@svvF134&=gGxw(Zb|33Bg^*kK!1gPYBqPR9dCE)h5wrlvHqXe(% zY0k9te4pAqQ^V!32pGu|3genU|fmlPqHupH5~o(RBoQ{4Mqk^E?k)V z#sbZfbACu(P|g5I4)_W+c-#pJ;P%R0Ne;@Xa=#!LJ$e>D8#l8cyC6LibR73e#v~`{ z&a>N)Y>WAFF?0ol%Z{z5mR< zu*M(#)|=ZRDb!FG3pstj(f}|6-mq+X`RDi|1Y-d)-Kk_wQd-)*N+ZiDub5kgNg#U!X#eiLu;jMu{#VJ}I)t-f0YH z3RDIC@X@i`i?%fbs!V_+jkUBeZv@Ede{DJ zP!mm_WQza;ee0@QWcr<3RZi5iD<>H6~4h zzMHV7THkltiI$baq~5(vjPHS$qLxa1px@?OhUw8r!QVg;YBekUrKN5w%KK=5KRwW> zP{ir;J$X_@Pq=Fx=RYLk*2)&j*9~+PH4OYi&pfQuu&W&pW*e`3eb3=liCKq^tM z$b1~#srk)529V2C2Qz7DrMmenFE~p^VRKt28JHlq+>mq)BSAznVmze0r z;4em@m3Id?AkLZJJgF`U5jW6^>PHDq%e#E{A=$ZxgGQ(-qVRkXvtS%1+)X&9f_V! z^>55MLL3T>l3oVHyDnAV!MCd(NKyJZJUKkA3|HK~0)&MO_+WA}W+znMC~!P`XuH*1 z*I4hR#1*LFLAuBI8K_IwX>Zq38Rh+|)Z!g20L>8JNJ@xDIk#(^wlfJwkmz=M@;LA@ zh=UDhN{2DC-5k1W1SQ3uNzY`@tSX(Wft&9qUqDa{*(Z%YC4IU3zi=zU8;o{x8Zmf# z_MHR))K$cNNqz~upGg&}{vbSC(Z4l>KOR|)9s`deYfDb3VL!C92Ij}l+T*u0!U{GOE!lT!Duem?{lqzn{vuh7R|CCCHRk0-Ut3%R#XE%{oX5Z zI}sthL@b<{`5DUunGS;|xsPFPl+=M0Js&m;G)@0g`8tH%lCwri{l3MiU{b}W+n8qLi zy8i^dZ>j45$_fay?fQF>PL$^3AtF^*SUG3J5PajoAjt@u$vU!F$|- z>~+Ynt-)FA$`!Sgz~S*u^=|n7S77r-uSyZ||1tTGEjk6vF=K>;#R-RUPlW#ZjY(6b zaPe}rXoahaovcb$jTe<0Zj_3j5RJNDnKT}ytm;=eMX1Hzbhi|M?3nmjS!up5*E)(g zEV)Gh3PPqIFtXrY)!yE3;_<7qUq9NaY!J3=oxsKUpv#Th;~glG1J!1c!<1RsK#h5` zX_Q@1WiY5*L`D2mb?mnn_Qu5RSw@ZG`v}7Bm%GfB+57VLt@7H`>`#o}QoVIlS-P$a zKlu#h4TD0PnZT-M(-NSyQwOcpC?$&OPlooANs!P{x}&il!cc6+HY)&*RM)`jro$C$$|6v zft=+xhcBX@?WJu0K6UatGom7m?vkW=V-_S6#cjV}J4rv%sa)sx~oQXaBgV?k%?n_2_gty1tChmmL( zi{;|NZVJ~d71VjWwiyi!JM?=4l2X8ZCH#s6ceDVR77dqY?-0`a#^cdZ_4QzhPne;- z2!V+;I!Ejgy1%*}gIp>PAGjHoB{2L&-6S68a!$V%K5 zAScU{-uJA1og6Z%A|Q|V(4BE({G%G&P}uDdX4B1|4_+%k&HS; zhLp4ao+lk@=ml~|Nm+{wGyxn$e*!t~5gUImuL*L#sPyB7K_`(>_+~qngk4l=68P;> zx!BE-j^F1szNffOd3GLSn;utLxs&Mq$TxW4Mno7eC82e6%?KUCyvVG~No_pZj8~D! z(5v2gL=xZI-)B^N>-zWOZTi0UuK;AC2-meYIo;C=m)pkeqmc)gP)I@wu69H-q3rq z)xZ%6ijlg~+As$N>|ZK%?B13y7^u@P4BV(9%Yx#o_q#R@ zSady1>oRj7Z1U}$K!xMX6JPHbK+cWPy&00O0!#o-BOZcKB!L`>fx&*Rs~$A%hF~7RU_V}$c+U=GRZ}vk>G;4{OTg3w^UZ5@;GSG@?+27J;9w~3pQcsjNwJ)=| z0bp5bdZAq7B^G|2&BoCCq@_w}m;%T$gIGhpn_3RKK*{lVgF#tJOh8L8a90lKpBawt zFGlr#JN^m2K8cGT0;w|HWtsU%18|vaGN_XXR-2y4RdsN&bK@IwO4?lexj}pWA*ckw zzOR-;)i?<9cr^#bdU{!bDo4pXGF^q(_dq@ZvNL@eP|H5*nXqsj>SFWp_gy3#6^i5v z^i$Tse0?+jXTCw^Il=GElyH+uzI%q?;)aDw#iKcQ4JGN~Q!>69##nD!q+c~DEXvmb zy;lp@FOl2JoJYUuMsH0u1s37Abbfw-%E`21KBpEQAT-ylFta>oD{%pW-AKqYM|+Fn z3UlMx6;OYlzO5L(WdFN3$O4qfpsfy8$GbY}YZt4eK=~XYV|ea{%A1b-Ox$B~znZgI zjbSHlyJ$h|Qd1;L@0aMiK+<~m?591HLVJAg9F&URm0Z=avdAJo7~&08nB%J+E9IkJ z=X6k0p25RyQt=xJvci%lhmk;?ll#zhLl`eQuTXjN9P}1t{C<{{2@O#JzuoX3aEs>K zmhWi$o0(kPEE%DBX*7!;Mqt}UjNnDTipQ10l|D_=zJQGMO$~BLQBs!3F!sha&|>rX zT&k|6WwsJ+LQ>>p&#wIx9WxWJ;>S-Pg;LHo?2$7K@1{{kGo@o?<}#-HZRV{uQ;ncV z;i_C#7$*=I4qA7_2{NPG$iPR7O#mp*FM&i~`MzzvCa4ykG5Qf#Msx2JE^&cDU6h%k zLG%}huMciHPClVq9kECH@9h%{PWD*Oym`m?9BBGYx^l&(`1lpW0^+^{SRk-7{$qw2 z*@M;bet(L%lyKeqf@c7W{|!Q|OUzAHbN{uj%m(esqX0OeSe-TI%O|la_iVkR^(f+u zMbpwWkg19X41n0$`8g@p3Q6;x^~J685<~L#3=8~@$5~15ioJgmLit+?KZIre9rZ-b8za}<{f5J)P!f_Wh(0Lamk39GwC^NfL0^m;kh5O zX`V-(S5i`T*sS>hw$8xVkFwLvG7x6MI=2?>ZZ)%-KiX2Zf*S&J?fGG zw6FIica2OBCUBwL{5%Qw8h*z!{Oe zOcl?gpZLAuwAT1|4_Q>M0#w-O9Q7&XeRemCyQEY z$aX9cXFIap8*3ZL3#?!6!=FNFGZaVQ_M3YPtxRYQB?j% zQ(0A=5K|wd6%zm`DC!-uaTQ6n~`u+Duuhx4!a%st8+h3aM+!|Tz6n6k8vh5GE zp5%fvfXkC6Q8KlN#Dos~^3LeWhU3vBkhOjm&_w5u`{zOxsHNxT&SZFyOhP!BxQ)yScuB*wV?UCvi{&e4Xa^)(YK*-))67~6RItEiKiFd=gED||yX=RFtr2=_a)t-UNcVUFXz0&oI^b_>|BPaz`=UO^) zTU~hB??1as^$9=wx?PPFtv(LJ%8%8AN83icQ_|kw78t?)55~SFD-&ZU)yfzKGtt} z#Bbxzd*BI(blruj=3yYZ!OIWt5_J0Kw|U6LPf^Ro4C8+5MN$5nJFW#dwAp&gdeS4u zQisTXyG?9+lKVQfLRvyvKudQ{YT!ByesD0Z=VMi+JsT9bs}5hAr2_pkdhRbv`ic#T zKM1l{a7hMtM4>^^@3(9!T4GH4F}?@jPoQg??wg;;$AH-1Jo2)SVV&uclg4EC6U81- zKSXF{mC22$_kC;*-{C0WEp+pK3Z<%Oq(4t8=Nm9y`nYrznP&!9y4w^LTfEw$c^RI4 zKh98FBA$B{SmYaGA=#SaW{Dl&?CnlRx`F@Q7Z z&3-mf_X`FEKI;dd&!uioPim)s?%Gw>yNX5$C2H1SrucJm^N7a^tKLBWj1CH}N1t?% zu!q<-W(-ZCD9pzTT9LB_S$B=S<0GTBF1Z5aH{2R6H4PzdbFPhFYyebzu2En8%fVTo z5Ubq|XvR_dYN3#z@)>g!3?DT!>$vDqjFr3tu*dlEN~LL}(C3`Qgg>wVlgYnPX5rE% zAQj>-vzK-%)FfSZa&fV6c(rEK-G3bj-mJq;A`!q#MuTU{Pc@X}jp_t32lh&Vi2MS9 zSWr3Q+B;M{w-lM&vmRjtGFJyI)y0ILeUYp(=-nKjGvww_CbZb1Sg3Y{lVX9SWUJ;VxRj%%IFurGhn#P92in=gFzu0sT_ zjo&EX;&xXMg63ZV%jh9u@&nCJG?$*XpU-myhp}<*b$0eOWXRI63LX}9UQNHbI-qK# zrIU4+P2pz0nrnfLbMq@dlgEJ}*@}-qOI#yIlXCRT3qFtzvBPhJ6?|NB)R~kg^>iyz zki8<|87bJR1khwHyp}M6U;0|+*NXf?)zk9MdN%2ghqO89=#9)>1Z09) z&?YRBksEZ@lKV^@XZ7-$QifC)mzPmnlM5&5bqANiiAp14#~sviuBSwFp8jGi@n);Y zf2e7Zlv{aZ9rM*mWYDVr>ryUgK1@>OPv%`{Sz(VCis;TSvE=8xm6dgiTbKBtUdBMd zUH0+Im4*PB&a%!9shy70gi-2{1i`s{M-zR-iW;G0$+C4IJ+s;C3xE!z$Vk;LlZVNA z(MFY0C+8UzqHgN`KBi_K@zzk3j4p>yArpUzB%`GNQn+2c26pi2ytF)!xfdl}&~ud| zrQ;073CUyIZUKl()n}koqL(}o$GF*5-QV@Ydv$45D@JVf+nRCfvBmYh^4_EO(m%go z$UP11(2qT;{T;QOZY7dv9fNAMF^i7%IK*@OxwIsiVHlLAo7d0REGfMAeYnY3>VSmD z#x25bEeA)kh!ejevA(9(n!_cI3VVA8Txi!6+|f>DG3!b+S)8w$qv03nmf#uOss}ICzAa7Xh zbv$)3RV)QzGb+KgO>B#ZoYp*~JYtRti@JdxUa3Q=b;@{5l?2M|w^)g^n7!AHGaT9~q1X69xb&0D3}R+YT&w@(yU(>hFr zRQGG4B~}7*w+Y-lJiYvd`aA~%53{rJ;ai@I zTYKg9!tv$pmX@9~%KWLAoYDB$9A`yEHAi0xJ2Aq}o+L$*B`yUyVsTy$x!A$e{taS!+yGvtx5an7C&n`i^x3D}%Cp~Dl z+NF9fp0QYcEU}2z{6s@*X7=T3z+Pmp@KG44Utv|xO`pRFUCwywb5{OJh*|v7J_b>9} zR>W8yy1aS)Iy43CIE+IXRiZ|6b`FZ`6Hp`UiXRu5^k*da3lp$AOMJ*z&z4&(JSNsI z8gDmPlyHg(8AQh>#}t-c+^x%%U!;$0VFc7>s1ELp=^x}-+V3|$>A}UqBTI(W{}o@Y zv|TmJD{&USp9y6IE-5N|FoM)X!eVHHhW3GKoT9F1R7-PvSI?+&#QD=1kDYv%UAYTq z?{2#Hx@d1oudILZRu`~1JV#<0QP-L&>Pn32z#Es1_r%i}$NE3gHw-H)Uuv@Un@&?8 zhl!ll#H#)=Kj(;@n-7X!?S;lzTfcD2koe{0hUG)2d-x7}yNzEwPEteH8Ectj$&ByV z9xW~hEffX?>dqY)p!7)d{>1ba{q#WH#URsSU?xpRSM@oF3t{PfuZ!uMtpvnH9$PL@ zX4t=6poI9eM^8~iSbH}Al=PZU5FOX)TOpocc@;sX)P8AeVdFe?(Wq*nu|Nn(Amp8H zyZ@gtJKWVw9PHd`Mo2d$hTT~;ZaoNnl=lqAcso3i%`Q0img7&Gyb=)xTD_g&JjTyw z*<%!G9~}pq`4Z`zP0BQ*w|al1lYa{8q&dZ&2l+1WUkE=}xSnXqF;;|oeJqj3u z8~xj+Nc-+0!+qj+DzMVQGJQAV9>y+x1Y1@HwV|2lMeo#J15kmE8(*)Zu!r!HHKr6_ zcnhh|RM?{)cOEJV5KyvmN9$lmsZR_x^J5D;eN~qV0o6of7r5)TyB%#!3v~-d#Fnxt8U8h)-l(7WPY zBqQ;9nf?GXo3qcRo2loNZqh+iHBl;~(vNn2xoRar{{GZyAv!WJbQoxD!|chNbni#W z=-F#(M)dEAI_9neup?6ESN(t)ns{Q0-@ik0?$`ftP{?P!xsHtzt{67(HLj&mI-6nk zRfTbPSI+2o;wpEuSke0`7sjnE)*0qstc1loLicsdc~M6Sdore53*yBOliL}EU&cOD zy^bi7GaO2Q`@R$;TA!(gs8N_0;sxqaVlENrf}!%AQw^dwND(7*5Ppi`e| zP?fUx(IlU>txaK7mBD4+JJXAEJc^7#jXn{@KmD3{jVilX({@8w0w&bRGc}v3@>c#0 z7uK@;RA}LGng1wr{{mJrdBHH z5^zn6%*#tB4b7I6fC`NRJl5Zv0YM1~z3JeU;K5ne#^>EH7C0V!*9qH`PiX5r_|eaQ z`o*JSKZEdVb}AP()t$4v_e-gv5mxY5qr46qD4bJ8)oTI|H4#A4AQ{zv{*MNCZC_|IFmq4C4Q<7_u=teSTX;Hqd1h&lhu@qPGvBEt3PC%% zpY=1~TnG0Q*~=7fXWpLkISY9|0ea08$eJQhhS{j`fb6YbzZ|?nWKst$G)Hn28GYJ# zo#@X(swz5VoDs_sdNNCgnXS9}$+#Xmsp8N2Y9+Y(6_>bKy65aMen%^pJZmDDrzoJ6 znHdhV_{yltK9osbX{mssRwqu%a38X>0(i;iljXrSmov%1*|H;lHQDDHf))!o>e z%T)wFIIBhlJ!oyGbnS~CRo1Zqrv%W&(Jz$Iq$8US@5ZsA?hP&v9S%`BjMaqQ@iVv5 zFoK|hR*4jZZkwL&jm_=#zBo}cdr*lm7oJ5g8>4GvkbeZ~>)=Oz%f^*zXA~mw`_;~V zB(2rLV!g2Mp-c~MLk-O3=NvLWNU;;^lTVDT1IFTgKc2rygdJ6QpCG=tcEEHWf!b|u zNkL=htRc=tjd%b$3AZakb4R4*Q)|3MBp8DwvhvS#!3LIFmDKe1v7SSffpr4jD>`r6 z^%nDt!ERbJ(aP5nK4+}W&%bLs54lN-Zq+{?U)?p6x0a7A(K1H%m3Qhk38JNHB{T<( zy*RTtl9f(DpZe?cz1xrGOaM&X?tvJ^KN>+1S_>bpwt`BumG7agEzMme1evR zq^{BpRiOT|t7=ucPfaub*9F;wdEooS|5TrZQ(`SXsA}D9Jw!Q-*TvO(>6i-G9!Rp< z|6=N_5x5g9m~*%zIfGcyw#&Ojd|;pQ^zj*51;vX(fq*8Be5}iQQc+skdJ-F7FwuN@ z#>z@3zLX<;FqEpZ@Uv8Ei&yAGhuvYDZMRD+baQo-~92a`4jyoIv(sU6DW;J!RYG<8Jvr0$)PvJ<{t%9 z5GA~dUK?W^zV)AigrtwhjS^O zSHX(wzD}7#N8Id;uAouSYoiJso%%qB{^Il@(!$;JhK93nDM)roq*QX3!%j#&^i&gbIdfOl8aldJv z)9p*wH;uae$-)a4&I?Ff2sW~V$)j4V1O}H67ijHNY6S&Mc8mf89eN8BhDeKf`GH5* zz$$1nt3gXN{t}CjuXZ=f;F`y0hV~i^bB=k7HMadl`2St)w%lYb6bLN8uiwPNm^o0~ zGsD!aGU`&VhMOcDoxdV0R}vRH->;Ashsyc2U=qgjo9T1D`(Tw(drcu<)4 zd`7$}wt0ylhQpAEW2UK=7Uhz{y3yALOA*g5-LP=u&ahv+&Wyr=7M&3+_`LhhlXc4} z8nrXtf*D5;&BBAHh1=)?f#aiYUBA_%9lS&p=5Y(uS+Qt-qUX{oowq*V$boEc45fLF z_tEI_Ljz%e*j*Xc{Sd-yP1y;mX=`X`*ul<1p4!*0-gUIeEgBr0tL+0|*$LBXfoc6a z!#19A>utGx_Gc02Yy0&lZ@_;u=y{WVp>th3Q;@$<#&Ph&D!va<2?>oBuzMLT{tN3( z82e!n#5HEk!-4P_YiF3urSE4}BI}KrdPMW_dc^BIm^dP8#&uz#EgkTHiRw}jCwIn+ zA0qV9!&U@tsZME{D*E@D{(o$}1zeO}&_29?fP#R92nZ-5AR$VJgi5z`g91yJbSzttRn-=q9xta@uLQO9&?&0ozcAp$&~!#jLyTg26%Do;+5TSldpYFqVQdIUhyulbM8`pg;ORbkGz|LDVBXKi z5HR4r&=pJ2sI~0Sls!Zl>mQ^vJ$cX1HEE?{Bjd^O7{qCWfJFq>{{}z?9&M zMg^JUbot7>Gac)ZbZIDZtYE09Npoo;gdhWNc9j`0qb-c!*ip{j&%X=L3QHNe;hvk z5ITMb=|I><1cT>i{e-2)qWQWAeHt=f!o0$a2Iq+a?C_<*@a=QKrh8iIQBfL3!zK82 za_rJ^2c@7uRbRCAE|?&vN+Z!UYa3wc;BrAv73U{Cq~qAnU5$#^tZ;^UNz6xXrxS2&6x+nZCG8hoii9= zI~p9vRpIfQKt?4XhZ(jtAjg?-L{0TKJlT6+on5;|1!(+J6^?VHsdjcpG;NgO4^OYl6LZr(IX}`^H#~%=ry|{F zXx6;joe8qv8)CmWcRvBdKF-JI*hX9f+f3@0?I^|WvgXmhfkA6TMb{g^?490oD$uIx zZ59_BjEPT1F@t!7v5&vTN%^m4ar5A`c!|}}hpzk~%lL9|!W6;jlZoS7WtD!8f|bTq zy!D43QQGB`GpEwzri*6o(aUL|@Gc9W4BxH{)a0@&#sScE2gqo-SwjE0WM>J6*o@Z< z`ZD+~=b&I!#)G7I5ve70rRb=&@vQ8@LsaiTVPRBQ7*poT$^iof6CPDe>95qaQ8oJqzb)#Kv2J(+)m3Y<6S}CH&)_u8_!}3BGy9_a zx^1q-?`(k(&DhMx>6u>--db|K&%x>NZTr{U$6B#j_ZS-Ie~Z&| zmE-nV+Lz|5*^Z)*PcwY%FjY9Y5cKG-YtuJZTQ(pBrBFd7*<=dP1qDFu5Qwr;n-gU^+|v-rZJ?!-Bh~0GE~EXu0%9qNy=6!mXe&5bjjvCS~D)RX(B~2NBozl%JGF zeCX>o3dsUkLYL@ha}3=nlfp}!LXSiP<5{Q_s7c*TFtnT&Oa-%7d@p?k_> zH85g6u{=oSg_wA+TjOH3p<(dG;A5kr@zB$>BowV{)%>B>&*u$Ihp-S&%S8X)dwBxH zODnS_9|;(pd@3Vuono@h)VXOXkIJr}=ogX6= zToD-^qe{HMFgA+~->pbKjl2SsVfGcmHi0GO?~S+(l^Jo1sjvt;?S0XRB6A<-ID zAN>Q(#XIU{)kuMj7saaSbaVxycN<-LfkUFPX;5&}hw=xSqRDW1)Y7u>zEkEe+~2+K z`VQJ9jfeZh`dr6U`)9B1gm)LYWILtbzkG#uGH=z%ejXGS=HH9qdd!a1pSu@U076kh zK)`$!#k5HAMjN-X>zTz!u_)XzsCU@F4qj|=HgZ(EpHj7!?o5|konNg{q^hQqp?mIO zl_d9jZr~kOPrRoO9Fvkqhped4f7G6pRh^@y9Z>4D#H8^Ac%{xi2M8g0#qQUn{)Y%JIm34Qn*sT**{hwtCRVO;?~m@T8$ueV~^1Op|`3$g=l_vPxnz( zLA09JUUg&Ll48`tk_=;HEkaPD0d*eIUqU6{TXG+^bH+4L;vP^+YMPst_6h{pzlMg4 zFYbQ zFw56)bs4x5*s~L%2gAc56f-#juH7}Mp4?EWozR@)Q^Y#b`v=aC_!su2S=STNa7ysj zkd3IU{H;&#(Hl`6U2^|~<56uTO>7tmy6#gkkRJh>q&{s2Ez>3^Cs&-u8v8*I#(s8B zf*ryKf;q10maN002~8tr)FP@cWqt}2p-bIzl_o{LZr&?;`26+WG1|N6GxgmOx#;gz zDt9(0^f=I#!SAwLSQbz1|!LImx8Ran~Q)9~~d^jamKhzO!&= zt!M6r;Bgh2idx9kvf~IA zG8GGVi;AQT_Y;8|klwyfVb7Cf_kDHHXd?m}f#pMFA8McHXvkQ!8j+3=Y;RdX z#&MbQ?(%V`KI{RVGfs=G<3L_yLZWJL0}bu!>I@S%f6>iY=23ynG_dhpZ4BBRJa|31 zFW1STQ)QP#nm{V#k|;HQ`^LRgKvm(f8VMdiki>oMa}*$oG|`|^O^82IJEdIX&^XIEZ-a_`~H1}&-QzonW%h4)!TO_o@BLIDS!y2 z_e@+jEFi7A;80%XZzT~bDv52pn^Q7xh`CcU zhSai4kO`HlL%$00JzWY(*|Z;m6Ag^M`M==ON46R0!I$IZ8A;YT^9{aE6sJBr#dH^y z#tOSa&8F}iNK}RhjtDGN&Sl~St@HeyCyung6wSQ{7ch!)dB^Ih<}xbDO21fBO>O0z_D~c*fPl}Q6C$Q6c7biHuLTt;Rdn6Y z@H%Y>wVhO7KMr0ORvc|N^f92M|L}j_gw_vKar44VFPrNPt!;?KZPw553zaHvaOfm zC}{&=knoWQ@p26OG$!nG2O;EvRK<_n&SPqiugfbJ8xCUJj(6~;Mg zt9BGfukGbEbo^6j7z%n#8{sn|Px|K;hzBd8J?qZ*sO ze12L#KL&!o{SN6oV50RWmQ?^6qK@s|$EDX@%ga31FtDu6e~Ck6jyU8M2faR@1EYmq z!t@gf}3$uAQWeNi8#en zOtdm043r|qO?W}c0c1<;!ZA1@#b_6G>$UT{wf zq*`8HP3;Zl=SOd*umTOZPL=sv6NcBVE-SO#cZah=aAY8TZxF|Y3U@YzAjwR?b?5DI zSBkhpDJl)totqzYNKeJuUDeySLQhJe3R)5ppBYw5`_sv1H|^|M=Jy&`=z$s5kkyH2 z$dwTPgbhGjGp7ayfiqr;P>2fmc2XG?bFG}Y_7>_yB7Pt3bw1Y9fJ|`ghDy=QsU!$4 zQqx=u5$jD>V{bft8I{5u!C=Z}m>xc6&a;X5J!=9Qz4JyQZ21IVCk5XZD6Pr}F}u#h zu3uXYzv&Jcdg*eIWh@}j5M7{?8QR~!HeQg>A9!6;!{DON{_&xM!aV^M4LI<>#3Ieg z#>>6C?mnh~S`2-9vvaV`D|pM^KsI#Q@+?F5zG(tjp*F9rukp?yJp7bX+S~o}nC+#R z)s}zxEb}F!iYn)nOBd6KIYWeD6jj7uyjZc7!;ny)+$RgWSKKCO#5p2tr z0>xDzI0|Z1#^kY09Lzt(J0n-Pqhl%!sSVvneYwrAkBIE%l~!%oQ;{L>r~mnf2ST)< zucXvv(BKt22CXtCp`X|m*_2omyl8b@`iiRyJ(n)vs|kf1`|}O9IwnK!@`7F9GBjfT zN&aq-{|h1gyN1gjY!GFR8IqM0_*5DrwOmz4;cU~>Uwsy3j;bu*f@)oeJ>!LlrG6u_ zoY8>M)=pZ~ul#vKqa^$M!Obc4^?!K4m)0`1p1tj(>~=q)4i~gFyNsSlvH_MEIi+Y$ zXRM}g%84$Q+^y`xPyXhAcDe5GfT{E_%mC>@wc~5)A)td3L~-8K=|Xxbpj%vnQT*#9 znXu!?`j5Eed`OL>PgkgEh;6g>i*YQ4t$hK+gNTv*_n2NG&~+pwsgTwck?;E_np2w% z_~PJwnoG4-5C<8v^he;ep(5EZu#*}N@F$M#G@FjPN_9zU$kDGAv zlm9L6@tP+&Eq2NUa4+p$8uu|`ywq*`xdZl|ZSSzpKSKZd{Esp&E`r5u#GBVHxwEs2 zMA0-6Fv9==C@)dF&hpd)?>3Ht;e;O^=4?5|vVka zbsx<3cH;d^I({Ry`@fO0E@xx=ZOBOyXMAi+8++V$q^l{Z{J8hO}Z-f$klq8mi-9%n}vfdf+6z+ zt*dZ?d5x6_u%SbgH33Hg4C96PcFFxr4xOxsM7{L%RN94Xx}nlzT4$`lztPG1e(q&u z!~!1G7<(b7Zoa2MF;Oirystg+8V>Nbd{;^FR@Sl8hU?tz#=SXq1thkwm&=I zLK{9~XA9J_wE*VP#70e7wT=n`Wb$KIFRPgmm$R$zOC(2+m6X2X$d~rx8tPp66KC< z12HGtwA%FzB(edKfP1~SN|<7KMzL#pJ`?{7aD?hAQ4tIvGoa95oQDxU@T9?*R% zXd^kC-lfG+89O`gVSBw&Yj;N#lo~M^YzFyKpoh5XPtYI$+=%cRnEu0<6ws4eP9}=W zrf$X|$%K;~$)^Vnx0~hsPyWKkyX3xYe`K~m zw3O0W84k!saaq~$CV1=bSmA+!eMlj2tHJ0X1ag4qlpETm1hRujh3QE#U~GNim*&CO zPxhxSmPwq708LCw^A$dNl(0B|r53HDH&dw$sLhJ3@^SL?E)p{k^7=~ArE znjJ@5*QqJ8L$jkKdw&1s@mNv5Q1)dITih7wakxLhrbZFgFxD&%d9a97(3F;uNlG!R zngn%t`saRNTo3Dmu{{}`HivRof~sal0FRO!gib-aQc{X@YLd&$QvF?8=zZZl8RPcO zNlV~L|6OR@QeK*)iulhCX=qgBBt7_FK z1bD5To#uGYTHa0lkxo%thHS`S&m==^8=>Vrl_#a6pkn_UKPw7YZ| zNSO2VdYdYJUJH8fRfBU+kb-uvK$nrJ+aQz!=E;%<(FpH!kZ`ZjQ1nq;4T`4xp0ip^ zP63~7{>Nhck2m2d$u$)%@kN5SjH<((2MgP)R%aYXF4s)KrxmXJcMoUjdz;+Qs_}Bm zHsT5NX&(WDrC|NZ>5z-su!8ubdBW8ZrRkw5pN6bYxz{WQ8$;5+S7@0hR5$?e^<|(j z4bhoR1UAQF{M2XUTT6lBG8^cYb9nIRh@1HN1MNX&044zO!^y2jvVxjR@6Ehahe;tO zBsZg49Pl59@)TILl_s6Ion7;iJyxCoE}vjqo1TcItEgAipu8SXAD1Ob<~ctu!nh0V zNYybo^QJn&>{>o=KHAI&VREEd!vyi%;xZi20W1#X!r-T$`V|3n2+%ol^quNhz>ip$ z;Wmfp&u#$H|4WBKcuEg3<}FrzdBd}fip%ln+CRSH{DGx+A--9kFFcB7ZGgP*h%Q?= z$9sGR&dJWsu6osE77GLK#ELm>*u`J|?pnc00AwnqbyD{5s6^hXn*+tsNU}6~pd9YI}4mO!)aEv5NIY^u?E-X^gwAyTp*J;+Ns$!AZ z_n|!}!06@`B>2k=M!{@h6<*>M;$45_ig~}nP=MK$Px{mCGH%6H2FBZljyz|xe zcE|VMy=Q&jI9#uC?4!*t37M>-#u_uB$fOvdvFPd^Uj|<@<{nn%otWJyFiEnzM8Tn| zn*%;td5dH(3ez4*!G&kMf0XeYOM0Argq35r!PDJ92tb z3UOp*#Z_$9pOA6^xv+nIvC~1;^xfY$RLqYjxUK|HXIU)Ih`KtYVSwkQvi<`vFSr6c z{dS|5((_0T9_FaBwa$rF;=8IN0L=OWoCGehhIO}LQ`yr^S74V`oO%u3{OBoyr8c{K zv*cr(_I27fKHO+BpFxYRyQzHDAz6cKyv9({vDccrtg~ouR^z=RbSx0a~+(KTh^OIyEd`>WEQ~*EDj;5XXqp5tHT%9nCuqRxH~P!$Oze@}~;@;c)CnWm9flg|?;k3rIT%VCH5 z^3waQX0PkX;_->goR@hLk&!;>qAp;^nJ{F#ah1f~)?a?fks`R=^>Io?MifI?OblD& z^m03WGB`@1(59d+g#G3ab@^1Ih~OA$nHop zB*bmr9rzWaPt1(f5b+g3OB@4rMfF|rKws`%`boZ%M1e>U;A(a>h-t9!EI?6EifZ*7 z@8xhpOSOh9?F~r|ck$x+&*>}$Qqk_FHo;gUZ%@Yt&Qb$l(gtu+NW5KYM}+w_CA7zf z4UJ2fp*xuswJGCknTMLm180v}vAL9201DPCCDy0DnZ&Ma#l&gW(b;<`N#_?_)k9YRBpJM?|R8*9O z^Sd+^qB3si++%PcF&Aj-Rhm=cf|${l9XcX-W9AcSHTV3(uAt{@~&eUS6&W&?({>j6OeeV5`1;mIq^KXsT6`wj$Ui0g8pnzgZl z_jl{RKUuyojK422JYc;c&5$imsl0DmY-F6VDdJotralAf40*N{eAN~Bor4y8-&dO zs(}!V1`{I-==Xw6hxAQ1$STTl2@=e{Q5hVJDis=D0c|B#)T^%JQZa!f|7O$KH2BSI zKX&Id1+jFmlq4Zdu>F{TfCi#!Dh@V?U zL`Jzz_kU;&Qh!^;n&lX5;I@l@x#9UruA68d56p8sS+|ewcd$>+pVnwKyk!Rx&Rgcd zjOZ+8bEBm~Gt#aN88=GS4Zq87l6jX?f2zlau; zjK1$S9)lmP!Kv|-ee&F~js1i`a0~+ByQG&v#3D<*`rk)QcAE0W@w_|vvfJ1P`dycs znJs3g3c8?ja$>W1jNL%RnV`o)4nr;gck(Z+;}gAM=Wxumvu7-WO<>xpUEq9 zN9_7%&MO`tOT{-haZwv#&&6?1&bWg}>D;zq-^*Z9tpn#;Sh+8&; z#bQL1&ekHX%s~0-;GKJSaz+i^^e1_3A`7OnKxsm}8&X`M9{Mh7bS%DFJ&1yBq%vd@ zloC~S*7$<+wl-!|`KH~|bt){>ZQ6Y5kI0Y{wQ>aU3v~XEBhc^sVT1mhjj6OxBwNbBO{kMobF2Ie(c6-;Thqra~R26h{$0N)a^aCvY5`R z$(rJoXq9MOEcuY2g=_I^_GW zlF=4KoKTa^heWAW7d5N7hXd-c5AR8nOl@)4` z8*(hxv>vZ8N(Lq_1i>P z5c!5iUNi@kk)85yO6j|{J1?K^gAFIyGavL{#HyUR;pr}G`~36U?aXg2UtuBqx0ukO ziYf!$MRy=JgU%K1Nx;!mmz{{5aJ4S-JabYi*wmguRiBgY(@huxg+0ML;W25s;jYcP ztX-f}+v$KX)il(YuPp^*jR!>n^ZdKY(BJ;x46bLFFcfkP1r6^3HCYEC>Z_OGsP3olE=jU$sTAhK$>cjxkSIes<@Tz+Y?g0oz zPyZ{2-#^H9wAUhE0lwzCfUhhH@WycYZC*&TGssdssY5)K`MP??vA0640ai~w%F*jk zZ#7w{)70BBUPUA5tSqM7tX)L+%uB2QrJF)ODVe3c*il*#8R9pAf8AMo(7aaeC*PnKF6-Ao$(2Qoy& zB*Qsx%~Hs5W`8+_gs6iO`U+#Tt&9pFK|F0-c=r)d_e#0SXu_-}OxESubt>#%h{QXW zYxD`YEX2+`*zk^gHUEc^!7rGsEF9biW|A4y){`~|9rt-SxZN*I-rFe7#i2SNh& zAK$LMm5GX#va0EU{}c5*q67u0t4Z2r<(9t*OzDKO0%P>(4e97W@=9TPDm!^#twlWZ z4%DK%MQ&kv(+nJxvgHPf$U@uyv0ElT_NX0{8~SkaK}VR0k7;zfnpXlj4$bHNK1jS0 zKh|lZvpq-z+dblwRZ=M07Hooul9S&ZHhTfK3xHl`RoEZVbMrj>xpL3;cl)F+7S3Vu zM7{=mj*vtJZQp^amm+~QLqK;lZ=M3A)Lzh6Me^`7oU;l~nstjJRS(ptfN%^vmfg(o z$6t`OO*C6FA1+l_2L-H=I#m`02Q-s#So^`+a8ryt=ju04T8lr`5MPf=HYDVk8kpz# z?bWUai9M^MAfF6sowEDV3VPP>cf3%6AJ-0K`y3Cw8Hn#@RH$`{@F3eL05#eIFj*5s zr*mjK*mR!uT>GE&FQ`78kgRfk(JS~}9lmS=V#c%qpHn<3@p9KNML@Cge^+$P$#!{b zExElwh7EFqtUgtdFiiVXL_`EZ4?d`^F*^rjesG=mc)HRusXH(Eoa`2NrLm#Det3{d zKle2-O|v!lAvCS+F@`!OE>@i}J~q}NqMCuKH3mb?Bihcd$Ss=Q*5dF|LGO#eO+K~C z)GrSnlwP}<OGGMQ1D4D3T zGaN14Uox$Ds#_XN-ZkH1ktFCkH}BJ_p6@&JGv&;t#OJ$H#WMr*UWa;>QoFeDJ?I-ZJ`LW^|gI> zWb0`4yuVPq2g`merLgDz6{F#=>h;?@)Ydr@%@e~`e4d7Ns1jC0tt(z?R+fLjCal`L zX*Dw25kWpWj9>QBPv_=08$ZjTuV1j2bStuQ**3W5{q-sXPgb5`9(hkks+dI2Yj^}? zsdnd$Y`XQq<6LHCOM^UOiPG!8xN1&03I;g|Sy@&Pa5 zeJ_a6{zRa_W^Zm+29mFsZnFvPUDj8C?rqR6@xtff;lX|a;xc39>b>7w#OrCee6lNW zvJ^T4*=pHLnMGRIah-hm=QNWm1Q*e1-Rl^P6S8^=c}+&sIi>A*WQ{b-*KY8SUyHBZ zFt~Zs0`~pQe+yXf>&v%C)oV-eHCalvf}(eA<&-au56k4;VY2$+m6|CACr%qml(4oH zMjErRf4{3S(eQOnt5L67e?9~6kK0NiMZ^Q4_XIxdHo?4!6Up_QIwYJ>12xmNnR3y* zPr97(3HZRv+u#e!!wUFUh?GBNTq*N_QWQdNLuclZ2kHG{1Cir}S9) zOMrIW&ZOX48+sG$WBDRTRI9u6ao0+NoE?T_L+ae`RA6l^eP!fPv^4+zhU&Ok6U-NL zJ0b9k^t$mPQF)c5OvH6UA`ZMp>R0zv0=K{XfgY};=T@Ks%^%CPS zqEIjbF&;|^6IRT;suJ>r&%aj?UfZcZAH#$tofQ=DiY)7<=`;Sp#xc}7Yfk)4JW6K0vcuNRlbp4dk`w@nm-GkcI8eE`L_b| zkwgLgvMyGQf_G;h@S~^{TN{L}Mi|!_6;cfRzJ0M4{dzjB;AyObF9`%ig>o0#XS_O> z`NAHJm#Q&)qi-i|7^zE z&1{O()0fg{qsIjc4@bz|Y)+eC?LPkwi9*0&O8-}1N@-$Gdp8t5l(#3kT{knZhkll1 zpr_aF%pt#dagCGiAxC324fkQuPYq!$*ir7 zw>+H=#NOi1$e;jBEZD-ESgVFTtE8Y{4{~Xnv9CIR1t#@Uc|Sd)cB>hfzi~Mo;ML-x z&%_dLMXsFRc5|h=k>>w?g27JMp6r=RLV`RSt4st+OgFcTbzlkRytjhVcOCly%WQv0 zk6WyJ-rx}%XOo{>s9k*WI!faf_Qu}|=6*}iX*8Vt>Z+0P90|BFf@eJlEcE|e-ztrC z*AAc&Y|ATqj?1M{?fAyhW<1pdq_5b5h((o7t8ME2;X8YM-dVNy7r?Qhi z6SBW8pR<6WPF~?!H;2MlghTIh2@Q2mE776VJzhYdm7baH%Cc6p3711E_m+tWhGgN5 zX7#lO|0!p(-F{i|4Bt{BW0JC_-S+Fs`02SX-oG3Z<&-YoJAn?U3Y#{ zoG^)=#%jssjsEZ=J)6%gjzez2CUE+)!j>!BQgtGL6iUOP&=@>EO^^EpQE@caKx;=r zRNj{T+ViqcZtxD$?~M~rz+`}H;GPt8LISVi;U`^R-O65jon$bP0}~{Ldk|TzE5Qnr zn;Q1_J#T(2FX(S1uH&qH&$CH zRkgOdA7M4HkMK42YiuSmDy{3A>4v#N4)FXTp15>4gHte~$Q>mEKS? zuM^QA-jue?bTPO^@R*zCWcNw5YCYaZFz62)wB2~6Fi|D^_qo0y&8wWJW15I%^o?Jb zZVXQIO))Yf=+%Fjw~Q&}6I=2LpOMAhd!;q_XLsew{j1k;YjHOrT(7Re%T9q!0>URN zbkj}5#?Tr{hTC+d5yz>^g}W@X`7!EDbkW5eN)uS|%}4U&fqFHJ(Y~SXywGUyY4fc) z!s;k!Wl-JdUH1kY;ydwDrq_v=3Zv=x( zBkik0$K0PxG6U52@brFV?M}KyFrXo#fAGD?fl1Zq=nlp8xXrcto|LOB#KJSJu1NMc zl)f>A-4WdZ9SJyWgWZ2d=hlKaH(0ob@SP8|tE|Hl!m;vOZ0>4yM4f2|ZtpV~^y6uv zP?7QOjgJB~w+L{A1AWAx8OE`y8Ry;>Gru|E$MNG&EIbuUf`4-3Z8^5=P7|--=M!q+ z7UA_(A9vm|-ngC^{gq=rsQHW#LTF~tXb80~n}XTK(FIt~)Z+N0 zd!Od!BYdU0t@GahvEJx35}vpP4->_CM60`qJ@)iAm=D`>X$i zv6@#=OLa!u|1-QW=o zm|I0Gs5ek))}D>`u1!UMDyJ9vj?Ux9ZJHOHV7+~Qp1yYE^#}QTF9i#!yMkX<+Kt{2 zEzf5d@$cweHuss)T zA?6-GFPr~k$bL@vmC&+Dx!CJnN#ye~*q4|3%#Pbq7xwOJr)iyt>`C%$Gld^Z^XseQ zS2FcdEAKMPX9=c5RVUP)>pXP}T5~vre@3MV=^{p`|x4qRwkwUhI=S`BB0Zef@kE^PvH1W3O@Rgi8 z8jQ@^%tUd78?htL-xROe61uJ6xTHl?SoxkJ4kpxMz*H zs7Hn#gvB8wPQ`{SXYtnSOM3^R`8vLCeg? zbfVI_8P2kRK+SIS_YqqrkjV^mWVN8*G=6OH&h8Pw>&yV$WLi5}+T0STp9wdmR4-g!B(-$Eqr?7kX`-Y#m-PhOkv`+<5u#fXuHS|K&uY5-}!8=SD_PZf7|Kx%DyS5vR3J z)~xL)?zr1;?3N>~I}j2}F4T!QZ$;R-qKm4JA1m`6^_j+-SNpnA&4@lt?GS&UWse#QTxjV`I}wGr;PJetLwv%2wFS@9LB8XRI%m ze-q!{`lXnQ9%jl<%jKaJLwH@dgYfgK#Q1(XTI|?fh0<#Iethl1q`{5R&2=y3RD|zT7`NK~Nj7+`P9hQ+Z?t|6B%xvMsqCRCqOo^_?pDj! z71P?-y5pA|Aj_SOKJ9%_?v+`T+<6<_so5xbot6cY2zE_G~p=GZ8$RL9W z=P*{5h}-O26>*&IEmwN1s789@)m6KwajD)9d+xrL*u~qm%q6i<-ahOZ8+~VFK7pD} zd`V8(CC{ZRf1Q@j66$m1tOv1tXH~aQ!}@i5E`j2BaEwXOU0$c&$37$UJ%5!`U0_*= zMx#kooJ+}Mq%4h{(YMhi`RgqQ4@Yu)I`J(H?OplGns5Vda`gw`4h|x-p$6%f!0-=L zHlpPZ;ak32R$X@wIvDlUaC|scB1q;$$X|_#IgI;mbv^u~BpYm@^lpc|dFsZigWj4l zPUNFVhNI$^l2S}YA2rsqsf23$*5@^aBECe1lh7Mb3MFIgmUQP%ci!%pUhBQm%Uu@N zJlK-j%Zf7Cxf{5|S=QSE8GbNl^Pv5W=%bP!j>!nXC&<#8w2z4>jhu|CmB>{-_^66# zz2;iB+3kWys#tgW7)DE`mt3jDK$p{_z`#3;vyc`QE?=jMG0$JT(3`=W*&lI8VmYhf<+1;e@K-)fc_S8mQyh28dB)U&FY|gUQhOE z`2+5yQc8$87Pe;kjXsAS;M$XJNvw@dO_3i;V%(;?du`TC#Gx+i&!0_Ih{urmG5MXf z?ArcmSD*~|bGMRha60>2QOAL}XDax_qhgsTDZp zD@%cBnrg5=PTsX@!H5hZU=9=LQ3a8k-#Qas)mxGrVFn(@X_KuPZYL)oP$;VbOj-5~ zCZhGLiQ%?q#QPyk1eUJ!(6wdUirpy~)g_F-^N17KLpp>j72foyaA;j7B1813ZfE-1 z!|$veF5(zH68YYO>ko$*WJ(iboraV7nR4AAiG>hwbWS!6LF+6GC^IoS_LcMB%W9~BVX6KsM z5Y4({6w14{F3cHi=J;}ANJH4{+ueg#Suk6jxs1e3Jb>QUS3iCHoGhJEhqoL_wsDtu zXzAqkQOEVx`VO7Q<7)^m?1|s&!@ru%eoRl zGym2!l+a$d}8e#(f7Z^A?={c@#7rb){#3z zD1&*Fk0UzBFNQOLbgcU|O^QX+5=zuA)rXh3M50gIL=Ir*1s_8}9KHnY{l;Vc znjy5zC6i2CNGnd{FEJ!_3z}W z)}&TEHSn^_(U;IK1=r9j)sFYCs@t)|YtahEpQ~n z43{H*{23P)glqC=ckd(b!-?leG=0h02IBzRq#i7U+wk z%ej`NPPEvJX6PuCi6~c+BUt#(>KTqOAg&}6_kcp@*^#|cD z`G^IPS<-j!+XZKVZH&TBdbzh8C$b@-rf7rx22=E!3G}yg5!Cd1{3m!WZEyMca5n+P zx(PHXhwcxoAXN^0kqrSlO~yKVj@iZ!!#%>w(I-{6E$+Ccykc{RARh4!)$+j@b0xg2 zuil0nJwPTjj}C}gJ((wpCN8sZY@=Wk9dGu&^smaBZm&1vZYU;R@f5a~%TfZ$cyQ(a z3K|6E`}Rr~XNC}pPClL_NeA*}3f2SD%Z#aPpf-fCiu&J2{t;N3B+0BKNfrUK$(J-- z4V)1|sJn|+fm#u1?XvY#e{N>KHnUrbl?!NuXEN=tnAsg>wg-Sk1I_Fj;zV1$iL*53 zv}i`CyUzyxCtd6TOfa*{&1|Td^)$16%%%m(IOU}k%n*=586=!0q5bIt~8&oHw;nAun}JH46v6)DTz{{S3E{9Vr3 zkytt;cd_CV;3nW0cV9wzO%2o*{=SNch=_RHF>sX& zHXYr>^U7I5%;+7IE}xY?{{hwQl=>zCC#3CV4fI`<>HBG^PVW9X@Od*E4vZpB#@7v) zPyE32q`Mau{x7w!sb-dtuRB|q*`Dh+v!LIr#rlbeh=_Gx z$Y=1rlijps*J0 zqwYSn5`z&D5fKp)5s|h-fm#uf#%oWkN4pa+19%#^x}qZz5fKp)5fPC#;eP?s%}D1P SC`6C|0000`_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ [#f2]_ - -Trajectory representation [#f1]_ ---------------------------------- - -Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. -By default, a spline interpolator is provided, but it's possible to support other representations. -The spline interpolator uses the following interpolation strategies depending on the waypoint specification: - -* Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. - -* Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. - -* Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ Hardware interface type [#f1]_ ------------------------------- @@ -114,141 +100,10 @@ Preemption policy [#f1]_ Only one action goal can be active at any moment, or none if the topic interface is used. Path and goal tolerances are checked only for the trajectory segments of the active goal. -When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. +When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. The trajectory is replaced in a defined way, see :ref:`trajectory replacement `. Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. -.. _parameters: - -Details about parameters -^^^^^^^^^^^^^^^^^^^^^^^^ - -joints (list(string)) - Joint names to control and listen to. - -command_joints (list(string)) - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. - -command_interface (list(string)) - Command interfaces provided by the hardware interface for all joints. - - Values: [position | velocity | acceleration] (multiple allowed) - -state_interfaces (list(string)) - State interfaces provided by the hardware for all joints. - - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. - -action_monitor_rate (double) - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). - - Default: 20.0 - -allow_partial_joints_goal (boolean) - Allow joint goals defining trajectory for only some joints. - - Default: false - -allow_integration_in_goal_trajectories (boolean) - Allow integration in goal trajectories to accept goals without position or velocity specified - - Default: false - -interpolation_method (string) - The type of interpolation to use, if any. Can be "splines" or "none". - - Default: splines - -open_loop_control (boolean) - Use controller in open-loop control mode: - - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - * It deactivates the feedback control, see the ``gains`` structure. - - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - - .. Note:: - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - - Default: false - -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 - -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. - -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. - - Default: 0.01 - -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. - - Default: 0.0 (not checked) - -constraints..trajectory (double) - Maximally allowed deviation from the target trajectory for a given joint. - - Default: 0.0 (tolerance is not enforced) - -constraints..goal (double) - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. - - Default: 0.0 (tolerance is not enforced) - -gains (structure) - Only relevant, if ``open_loop_control`` is not set. - - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. - This structure contains the controller gains for every joint with the control law - - .. math:: - - u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), - the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. - -gains..p (double) - Proportional gain :math:`k_p` for PID - - Default: 0.0 - -gains..i (double) - Integral gain :math:`k_i` for PID - - Default: 0.0 - -gains..d (double) - Derivative gain :math:`k_d` for PID - - Default: 0.0 - -gains..i_clamp (double) - Integral clamp. Symmetrical in both positive and negative direction. - - Default: 0.0 - -gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_{ff}` of velocity - - Default: 0.0 - -gains..normalize_error (bool) - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - 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. Use this for revolute joints without end stop, - where the shortest rotation to the target position is the desired motion. - - Default: false - .. _ROS 2 interface: @@ -281,7 +136,7 @@ There are two mechanisms for sending trajectories to the controller: * via action, see :ref:`actions ` * via topic, see :ref:`subscriber ` -Both use the ``trajectory_msgs/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. +Both use the ``trajectory_msgs/msg/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. For further information on the message format, see :ref:`trajectory representation `. .. _Actions: @@ -324,17 +179,27 @@ Services Query controller state at any future time -Specialized versions of JointTrajectoryController (TBD in ...) +Specialized versions of JointTrajectoryController -------------------------------------------------------------- +(TBD in ...) -The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). +The controller types are placed into namespaces according to their command types for the hardware (see :ref:`controllers`). The following version of the Joint Trajectory Controller are available mapping the following interfaces: * position_controllers::JointTrajectoryController +Further information +-------------------------------------------------------------- + +.. toctree:: + :titlesonly: + + Trajectory Representation + joint_trajectory_controller Parameters + + .. rubric:: Footnote .. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ -.. [#f2] Adolfo Rodriguez: `Understanding trajectory replacement `_ From cc4c221a680294ffefc7fb061a1d25341823226a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 31 Jul 2023 14:47:00 -0500 Subject: [PATCH 327/524] Small improvement in remapping (#393) Co-authored-by: Bence Magyar --- .../src/joint_trajectory_controller.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4b3a73c3a9..616af7a648 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1223,8 +1223,12 @@ void JointTrajectoryController::sort_to_local_joint_order( get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } - std::vector output; - output.resize(mapping.size(), 0.0); + static std::vector output(dof_, 0.0); + // Only resize if necessary since it's an expensive operation + if (output.size() != mapping.size()) + { + output.resize(mapping.size(), 0.0); + } for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index]; From 5c0327d4e72b164aaa4a365a40d7a3fa913c0f97 Mon Sep 17 00:00:00 2001 From: Ben Holden <94607409+bobbleballs@users.noreply.github.com> Date: Tue, 1 Aug 2023 16:24:34 +0100 Subject: [PATCH 328/524] [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (#533) * [DiffDiveController] Changed the namespace appending from '/' to '_' with optional tf_frame_prefix parameter. If not set it will not append anything. * [DiffDriveController] Add option to enable/disable TF prefix. On by default. If prefix not set and is enabled then namespace is used by default. --- .../src/diff_drive_controller.cpp | 36 ++- .../src/diff_drive_controller_parameter.yaml | 10 + .../test/test_diff_drive_controller.cpp | 221 ++++++++++++++++++ 3 files changed, 255 insertions(+), 12 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 20736fda09..23097c0df2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -392,23 +392,35 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); - std::string controller_namespace = std::string(get_node()->get_namespace()); - - if (controller_namespace == "/") - { - controller_namespace = ""; - } - else + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) { - controller_namespace = controller_namespace + "/"; + if (params_.tf_frame_prefix != "") + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + } + + if (tf_prefix == "/") + { + tf_prefix = ""; + } + else + { + tf_prefix = tf_prefix + "/"; + } } - const auto odom_frame_id = controller_namespace + params_.odom_frame_id; - const auto base_frame_id = controller_namespace + params_.base_frame_id; + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = controller_namespace + odom_frame_id; - odometry_message.child_frame_id = controller_namespace + base_frame_id; + odometry_message.header.frame_id = odom_frame_id; + odometry_message.child_frame_id = base_frame_id; // limit the publication on the topics /odom and /tf publish_rate_ = params_.publish_rate; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index f909b27e8b..5d20970cab 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -39,6 +39,16 @@ diff_drive_controller: default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", } + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } odom_frame_id: { type: string, default_value: "odom", diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index adf4e79afc..edaaa84e4b 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -68,6 +68,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr } return false; } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; + } }; class TestDiffDriveController : public ::testing::Test @@ -244,6 +255,216 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) SizeIs(left_wheel_names.size() + right_wheel_names.size())); } +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's instead of the namespace*/ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the + * frame id's */ + ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); +} + TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { const auto ret = controller_->init(controller_name); From 3aa0b5ca0e2d4fdefde41d2ff2d4fd5624a89b5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 2 Aug 2023 15:03:56 +0200 Subject: [PATCH 329/524] [CI] Bump ubuntu version of ament_lint jobs (#727) --- .github/workflows/ci-ros-lint.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 9e08bec730..85afe2dc6f 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -5,11 +5,13 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: linter: [cppcheck, copyright, lint_cmake] + env: + AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@0.6.2 From 8b02e9fd7f470cebc4fa9b15424e96977ca79caf Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 2 Aug 2023 14:28:17 +0100 Subject: [PATCH 330/524] Bump ros-tooling/setup-ros from 0.6.2 to 0.7.0 (#717) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index b7698dbbe4..3d8a7431c0 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 85afe2dc6f..3d102b53a2 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 99a4ed66a5..d5f836f026 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 882b634871ab5cdd399153ad9ada36e192be3249 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Aug 2023 15:11:59 +0100 Subject: [PATCH 331/524] tf_frame_prefix should be off by default (#728) --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 5d20970cab..f3acfcf3c8 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -41,7 +41,7 @@ diff_drive_controller: } tf_frame_prefix_enable: { type: bool, - default_value: true, + default_value: false, description: "Enables or disables appending tf_prefix to tf frame id's.", } tf_frame_prefix: { From c37dfe63ec2f8929a14e185784c8e39417df5284 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Aug 2023 16:25:45 +0100 Subject: [PATCH 332/524] Revert "tf_frame_prefix should be off by default (#728)" (#729) This reverts commit 882b634871ab5cdd399153ad9ada36e192be3249. --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index f3acfcf3c8..5d20970cab 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -41,7 +41,7 @@ diff_drive_controller: } tf_frame_prefix_enable: { type: bool, - default_value: false, + default_value: true, description: "Enables or disables appending tf_prefix to tf frame id's.", } tf_frame_prefix: { From 29fc1c32f15e660c95115f0e6263e4c199248181 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 4 Aug 2023 20:20:02 +0100 Subject: [PATCH 333/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 5 +++++ 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 | 8 ++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 19 files changed, 68 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index f2cb336a5d..a95ddb5426 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8f5804fbe5..2949cc9f10 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix out of bound access in admittance controller (`#721 `_) +* Contributors: Abishalini Sivaraman + 3.12.0 (2023-07-18) ------------------- * Activate AdmittanceControllerTestParameterizedInvalidParameters (`#711 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index c9e4604a07..1d42125c3e 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1df6139577..69e8f2981b 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (`#533 `_) +* Contributors: Ben Holden, Bence Magyar + 3.12.0 (2023-07-18) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 1134b1cc09..7489bbdffe 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7a420bb146..f884fdb0e9 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.12.0 (2023-07-18) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 7e33097a52..d7960a5d5d 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.12.0 (2023-07-18) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 797fc118ed..5ed71b34e9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9920deb233..2cef7bbe7b 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.12.0 (2023-07-18) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index a33b0cc011..cd36386655 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.12.0 (2023-07-18) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c2e3fa332d..4ff9cbbfc9 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Small improvement in remapping (`#393 `_) +* [JTC] Update trajectory documentation (`#714 `_) +* [JTC] Reject messages with effort fields (`#699 `_) (`#719 `_) +* [Doc] Fix links (`#715 `_) +* Contributors: Andy Zelenak, Bence Magyar, Christoph Fröhlich + 3.12.0 (2023-07-18) ------------------- * Remove reactivation test from ROS 1 diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e5c55a3569..cb5e519cc4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1cc86b0d0e..75bf98c565 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 73e59702f9..d0080d7ea8 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 75d8985836..4fc80aa5f8 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.12.0 (2023-07-18) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 88400b4e3b..408df542dd 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update ci-ros-lint.yml and copyright format (`#720 `_) +* Contributors: Christoph Fröhlich + 3.12.0 (2023-07-18) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index ce14946944..fec2d8782c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 70b91d59b8..88b6ff647d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c832892dd9..7f9643a50b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.12.0 (2023-07-18) ------------------- From 83e415c0f00512398df3c3a508f738fc76a7ecda Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 4 Aug 2023 20:21:17 +0100 Subject: [PATCH 334/524] 3.13.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 +- 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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a95ddb5426..b4c1815bdc 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index dab9499c7d..d4c6b3b3b4 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.12.0 + 3.13.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 2949cc9f10..5f85999abd 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- * Fix out of bound access in admittance controller (`#721 `_) * Contributors: Abishalini Sivaraman diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 83ca1f1b2d..966c268d93 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.12.0 + 3.13.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 1d42125c3e..e100f537f6 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index d6a971ab6d..42693418f6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.12.0 + 3.13.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 69e8f2981b..7fdcf063e2 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.13.0 (2023-08-04) +------------------- * [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (`#533 `_) * Contributors: Ben Holden, Bence Magyar diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 34d236a9eb..80d3f6334e 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.12.0 + 3.13.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 7489bbdffe..4b223619e0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index c3280f9c9b..852cf77829 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.12.0 + 3.13.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 f884fdb0e9..d9c0f2de24 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index c89df2b397..b2c04e6a57 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.12.0 + 3.13.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 d7960a5d5d..324eead77a 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index de09d505ba..25895a4696 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.12.0 + 3.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5ed71b34e9..506aaa48ec 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 3e48f395af..c977a0ed36 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.12.0 + 3.13.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2cef7bbe7b..ef997c8733 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 4ba9bb7a55..41c087f3ca 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.12.0 + 3.13.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 cd36386655..9a5c4efc30 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 89a23fc15b..fd5deab3e7 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.12.0 + 3.13.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 4ff9cbbfc9..e2b1c6f613 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.13.0 (2023-08-04) +------------------- * Small improvement in remapping (`#393 `_) * [JTC] Update trajectory documentation (`#714 `_) * [JTC] Reject messages with effort fields (`#699 `_) (`#719 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 63774d5c2e..08a5392977 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.12.0 + 3.13.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 cb5e519cc4..15242a2018 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index ef214c3563..1259239ac5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.12.0 + 3.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 75bf98c565..04ed272d86 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 1bfa80d078..560ea5df88 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.12.0 + 3.13.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 d0080d7ea8..d84a55f26a 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index bc0fab74b0..02af7f67b8 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.12.0 + 3.13.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 c90806a128..6aceaa2cc2 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.12.0", + version="3.13.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 4fc80aa5f8..3683d03c16 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 1aa9b0df6d..c2541f3a96 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.12.0 + 3.13.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 844e8a51d8..701c285e61 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.12.0", + version="3.13.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 408df542dd..5bf04ede76 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.13.0 (2023-08-04) +------------------- * Update ci-ros-lint.yml and copyright format (`#720 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 2b8004356a..1da3cfa9df 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.12.0 + 3.13.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 fec2d8782c..a40f09e557 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 91f6fb5a19..9cc16d3b60 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.12.0 + 3.13.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 88b6ff647d..8d3c61ff6b 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.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e24d2f97c2..26083760d1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.12.0 + 3.13.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 7f9643a50b..b9fe5f9553 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.13.0 (2023-08-04) +------------------- 3.12.0 (2023-07-18) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d861c91806..3551021f82 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.12.0 + 3.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2e7492e8c47c06de31232481c686d2215248fd83 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 15 Aug 2023 13:37:52 +0200 Subject: [PATCH 335/524] [JTC] Explicitly set hold position (#558) --- .../doc/parameters.rst | 6 + joint_trajectory_controller/doc/userdoc.rst | 3 +- .../joint_trajectory_controller.hpp | 7 +- .../trajectory.hpp | 3 + .../src/joint_trajectory_controller.cpp | 146 +++++++++++------ ...oint_trajectory_controller_parameters.yaml | 5 + .../src/trajectory.cpp | 5 + .../test/test_trajectory_actions.cpp | 149 ++++++++---------- .../test/test_trajectory_controller.cpp | 88 +++++++---- .../test/test_trajectory_controller_utils.hpp | 85 +++++++++- 10 files changed, 331 insertions(+), 166 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 40aa8e6ba1..d5ddb831ca 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -57,6 +57,12 @@ open_loop_control (boolean) Default: false +start_with_holding (bool) + If true, start with holding position after activation. Otherwise, no command will be sent until + the first trajectory is received. + + Default: true + allow_nonzero_velocity_at_trajectory_end (boolean) If false, the last velocity point has to be zero or the goal will be rejected. diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index ccdef8a8e6..750e47d255 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -104,7 +104,6 @@ When an active action goal is preempted by another command coming from the actio Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. - .. _ROS 2 interface: Description of controller's interfaces @@ -161,7 +160,7 @@ Subscriber [#f1]_ Topic for commanding the controller The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. -The controller's path and goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. +The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held. Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. 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 dfbdf7436e..10b0f5923b 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 @@ -181,7 +181,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; std::shared_ptr traj_msg_home_ptr_ = nullptr; @@ -201,6 +200,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp_action::Server::SharedPtr action_server_; RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + realtime_tools::RealtimeBuffer rt_has_pending_goal_; ///< Is there a pending action goal? rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); @@ -243,11 +243,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); + std::shared_ptr set_hold_position(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_active_trajectory(); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 230d0198f7..d2d0dc9dbd 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -130,6 +130,9 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_trajectory_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_nontrivial_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr get_trajectory_msg() const { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 616af7a648..ee9b0bf816 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -154,17 +154,21 @@ controller_interface::return_type JointTrajectoryController::update( } }; + // don't update goal after we sampled the trajectory to avoid any racecondition + const auto active_goal = *rt_active_goal_.readFromRT(); + // Check if a new external message has been received from nonRT threads auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) + // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) + if ( + current_external_msg != *new_external_msg && + (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity traj_external_point_ptr_->update(*new_external_msg); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; } // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not @@ -182,29 +186,28 @@ controller_interface::return_type JointTrajectoryController::update( state_current_.time_from_start.set__sec(0); read_state_from_hardware(state_current_); - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + // currently carrying out a trajectory. + if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) + if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; if (params_.open_loop_control) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_) - ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + const bool valid_point = traj_external_point_ptr_->sample( + time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { @@ -212,7 +215,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; double time_difference = 0.0; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) @@ -239,7 +242,7 @@ controller_interface::return_type JointTrajectoryController::update( if (default_tolerances_.goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; time_difference = time.seconds() - traj_end.seconds(); @@ -296,7 +299,6 @@ controller_interface::return_type JointTrajectoryController::update( last_commanded_state_ = state_desired_; } - const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { // send feedback @@ -312,20 +314,20 @@ controller_interface::return_type JointTrajectoryController::update( // check abort if (tolerance_violated_while_moving) { - set_hold_position(); auto result = std::make_shared(); - - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; + rt_has_pending_goal_.writeFromNonRT(false); - // check goal tolerance + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } + // check goal tolerance else if (!before_last_point) { if (!outside_goal_tolerance) @@ -336,32 +338,42 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; + rt_has_pending_goal_.writeFromNonRT(false); RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } else if (!within_goal_time) { - set_hold_position(); auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + RCLCPP_WARN( get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", time_difference); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied or violated within the goal_time_tolerance } } - else if (tolerance_violated_while_moving) + else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { - set_hold_position(); + // we need to ensure that there is no pending goal -> we get a race condition otherwise + RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } } } @@ -567,17 +579,16 @@ void JointTrajectoryController::query_state_service( const auto active_goal = *rt_active_goal_.readFromRT(); response->name = params_.joints; trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; - if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) + if (has_active_trajectory()) { TrajectoryPointConstIter start_segment_itr, end_segment_itr; - response->success = (*traj_point_active_ptr_) - ->sample( - static_cast(request->time), interpolation_method_, - state_requested, start_segment_itr, end_segment_itr); + response->success = traj_external_point_ptr_->sample( + static_cast(request->time), interpolation_method_, state_requested, + start_segment_itr, end_segment_itr); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { - if (end_segment_itr == (*traj_point_active_ptr_)->end()) + if (end_segment_itr == traj_external_point_ptr_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -916,7 +927,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::shared_ptr()); subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; // Initialize current state storage if hardware state has tracking offset read_state_from_hardware(state_current_); @@ -933,13 +943,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } + // Should the controller start by holding position after on_configure? + if (params_.start_with_holding) + { + add_new_trajectory_msg(set_hold_position()); + } + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { - // TODO(anyone): How to halt when using effort commands? for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) @@ -953,6 +968,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( joint_command_interface_[1][index].get().set_value(0.0); } + if (has_acceleration_command_interface_) + { + joint_command_interface_[2][index].get().set_value(0.0); + } + + // TODO(anyone): How to halt when using effort commands? if (has_effort_command_interface_) { joint_command_interface_[3][index].get().set_value(0.0); @@ -968,6 +989,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; + traj_external_point_ptr_.reset(); + traj_home_point_ptr_.reset(); + traj_msg_home_ptr_.reset(); + return CallbackReturn::SUCCESS; } @@ -978,7 +1003,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( if (traj_home_point_ptr_ != nullptr) { traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; + traj_external_point_ptr_ = traj_home_point_ptr_; } return CallbackReturn::SUCCESS; @@ -1009,7 +1034,6 @@ bool JointTrajectoryController::reset() // iterator has no default value // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; traj_external_point_ptr_.reset(); traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); @@ -1102,17 +1126,17 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal && active_goal->gh_ == goal_handle) { - // Controller uptime - // Enter hold current position mode - set_hold_position(); - - RCLCPP_DEBUG( + RCLCPP_INFO( get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled + rt_has_pending_goal_.writeFromNonRT(false); auto action_res = std::make_shared(); active_goal->setCanceled(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + // Enter hold current position mode + add_new_trajectory_msg(set_hold_position()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -1120,6 +1144,9 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { + // mark a pending goal + rt_has_pending_goal_.writeFromNonRT(true); + // Update new trajectory { preempt_active_goal(); @@ -1408,7 +1435,7 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - set_hold_position(); + add_new_trajectory_msg(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); @@ -1417,13 +1444,35 @@ void JointTrajectoryController::preempt_active_goal() } } -void JointTrajectoryController::set_hold_position() +std::shared_ptr +JointTrajectoryController::set_hold_position() { - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); + // Command to stay at current position + trajectory_msgs::msg::JointTrajectory current_pose_msg; + current_pose_msg.header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + current_pose_msg.joint_names = params_.joints; + current_pose_msg.points.push_back(state_current_); + current_pose_msg.points[0].velocities.clear(); + current_pose_msg.points[0].accelerations.clear(); + current_pose_msg.points[0].effort.clear(); + if (has_velocity_command_interface_) + { + // ensure no velocity (PID will fix this) + current_pose_msg.points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // ensure no acceleration + current_pose_msg.points[0].accelerations.resize(dof_, 0.0); + } + if (has_effort_command_interface_) + { + // ensure no explicit effort (PID will fix this) + current_pose_msg.points[0].effort.resize(dof_, 0.0); + } - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); + return std::make_shared(current_pose_msg); } bool JointTrajectoryController::contains_interface_type( @@ -1468,6 +1517,11 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::has_active_trajectory() +{ + return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 5627f1d8f5..1112c3304b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -66,6 +66,11 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } + start_with_holding: { + type: bool, + default_value: true, + description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", + } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: true, diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 06b3ca6e9b..fae4c41ff9 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -351,4 +351,9 @@ rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_ bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } +bool Trajectory::has_nontrivial_msg() const +{ + return has_trajectory_msg() && trajectory_msg_->points.size() > 1; +} + } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fdea551c30..2c28319d52 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -224,15 +224,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::shared_future gh_future; // send goal + std::vector point_positions{1.0, 2.0, 3.0}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + point.positions = point_positions; points.push_back(point); @@ -243,12 +242,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectHoldingPoint(point_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) @@ -265,24 +265,21 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -293,12 +290,12 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(1)); } /** @@ -318,15 +315,14 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) std::shared_future gh_future; // send goal + std::vector> points_positions{{{1.0, 2.0, 3.0}}}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + point.positions = points_positions.at(0); points.push_back(point); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -338,12 +334,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(0)); } /** @@ -370,24 +366,21 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -400,12 +393,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -417,29 +410,28 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> immediate state tolerance fail + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); std::shared_future gh_future; // send goal + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.0); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.1); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -452,15 +444,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -503,15 +493,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding the last received goal + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(4.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(5.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(6.0, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -523,13 +511,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; // send goal { @@ -553,15 +539,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -601,19 +585,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - const double prev_pos1 = joint_pos_[0]; - const double prev_pos2 = joint_pos_[1]; - const double prev_pos3 = joint_pos_[2]; + std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); - } + // it should be holding the last position, + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 375cdc0346..6cdf324596 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -285,44 +285,38 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // first update traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - // wait so controller process the second point when deactivated + // wait for reaching the first point + // controller would process the second point when deactivated below traj_controller_->update( rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); - // deactivated - state = traj_controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - - const auto allowed_delta = 0.05; + EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_NEAR(points.at(0).at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(2), joint_pos_[2], COMMON_THRESHOLD); } - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); - } + // deactivate + std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPointDeactivated(deactivated_positions); - // reactivated - // wait so controller process the third point when reactivated + // reactivate + // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(); + + ActivateTrajectoryController(false, deactivated_positions); state = traj_controller_->get_state(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // TODO(christophfroehlich) add test if there is no active trajectory after - // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController) + // it should still be holding the position at time of deactivation + // i.e., active but trivial trajectory (one point only) + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPoint(deactivated_positions); executor.cancel(); } @@ -424,6 +418,45 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if hold on startup is deactivated + */ +TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); + SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup without start_with_holding being set, we expect no active trajectory + ASSERT_FALSE(traj_controller_->has_active_traj()); + + executor.cancel(); +} + +/** + * @brief check if hold on startup + */ +TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); + SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup with start_with_holding being set, we expect an active trajectory: + ASSERT_TRUE(traj_controller_->has_active_traj()); + // one point, being the position at startup + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); + + executor.cancel(); +} + // Floating-point value comparison threshold const double EPS = 1e-6; /** @@ -1156,7 +1189,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; - std::cout << "Sending old trajectory" << std::endl; publish(time_from_start, points_new, new_traj_start); waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5ec0f4f29e..87324d61b0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/trajectory.hpp" namespace { @@ -123,6 +124,13 @@ class TestableJointTrajectoryController bool is_open_loop() { return params_.open_loop_control; } + bool has_active_traj() { return has_active_trajectory(); } + + bool has_trivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -224,7 +232,9 @@ class TrajectoryControllerTest : public ::testing::Test ActivateTrajectoryController(separate_cmd_and_state_values); } - void ActivateTrajectoryController(bool separate_cmd_and_state_values = false) + void ActivateTrajectoryController( + bool separate_cmd_and_state_values = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -258,14 +268,14 @@ class TrajectoryControllerTest : public ::testing::Test // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_POS_JOINTS[i]); + 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.emplace_back(acc_cmd_interfaces_.back()); 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_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()); @@ -426,6 +436,75 @@ class TrajectoryControllerTest : public ::testing::Test return state_msg_; } + void expectHoldingPoint(std::vector point) + { + // it should be holding the given point + // i.e., active but trivial trajectory (one point only) + EXPECT_TRUE(traj_controller_->has_trivial_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + + void expectHoldingPointDeactivated(std::vector point) + { + // it should be holding the given point, but no active trajectory + EXPECT_FALSE(traj_controller_->has_active_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + std::string controller_name_; std::vector joint_names_; From af54985bc09121d4ca5d45fba68effc279cb2689 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 15 Aug 2023 13:40:35 +0200 Subject: [PATCH 336/524] Use tabs (#743) Co-authored-by: Bence Magyar --- forward_command_controller/doc/userdoc.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index ee0af57d7b..a91479c9af 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -21,12 +21,12 @@ Parameters This controller uses the `generate_parameter_library `_ to handle its parameters. -forward_command_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^ + .. tabs:: + + .. group-tab:: forward_command_controller -.. generate_parameter_library_details:: ../src/forward_command_controller_parameters.yaml + .. generate_parameter_library_details:: ../src/forward_command_controller_parameters.yaml -multi_interface_forward_command_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + .. group-tab:: multi_interface_forward_command_controller -.. generate_parameter_library_details:: ../src/multi_interface_forward_command_controller_parameters.yaml + .. generate_parameter_library_details:: ../src/multi_interface_forward_command_controller_parameters.yaml From 5178503f1044cc988663c196c32a0039c72418b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 15 Aug 2023 13:40:52 +0200 Subject: [PATCH 337/524] Remove wrong description (#742) --- joint_trajectory_controller/doc/userdoc.rst | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 750e47d255..b9d8e274cf 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -178,17 +178,6 @@ Services Query controller state at any future time -Specialized versions of JointTrajectoryController --------------------------------------------------------------- -(TBD in ...) - -The controller types are placed into namespaces according to their command types for the hardware (see :ref:`controllers`). - -The following version of the Joint Trajectory Controller are available mapping the following interfaces: - -* position_controllers::JointTrajectoryController - - Further information -------------------------------------------------------------- From 6b73cf56d20b8d85db9ea54775765e67aef014a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 16 Aug 2023 15:35:57 +0200 Subject: [PATCH 338/524] [JTC] Fix typos, implicit cast, const member functions (#748) --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- ...oint_trajectory_controller_parameters.yaml | 4 ++-- .../test/test_trajectory_controller.cpp | 6 ++--- .../test/test_trajectory_controller_utils.hpp | 24 +++++++++---------- 5 files changed, 19 insertions(+), 19 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 10b0f5923b..1df557d029 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 @@ -249,7 +249,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool reset(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool has_active_trajectory(); + bool has_active_trajectory() const; using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ee9b0bf816..9c5ee68132 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1517,7 +1517,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } -bool JointTrajectoryController::has_active_trajectory() +bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 1112c3304b..5b8a8d14f2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -86,7 +86,7 @@ joint_trajectory_controller: i: { type: double, default_value: 0.0, - description: "Intigral gain for PID" + description: "Integral gain for PID" } d: { type: double, @@ -96,7 +96,7 @@ joint_trajectory_controller: i_clamp: { type: double, default_value: 0.0, - description: "Integrale clamp. Symmetrical in both positive and negative direction." + description: "Integral clamp. Symmetrical in both positive and negative direction." } ff_velocity_scale: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6cdf324596..4149058c77 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1531,9 +1531,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co // set command values to NaN for (size_t i = 0; i < 3; ++i) { - joint_pos_[i] = 3.1 + i; - joint_vel_[i] = 0.25 + i; - joint_acc_[i] = 0.02 + i / 10.0; + 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; } SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 87324d61b0..8a22f986aa 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -102,31 +102,31 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } - trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; } - bool has_position_state_interface() { return has_position_state_interface_; } + bool has_position_state_interface() const { return has_position_state_interface_; } - bool has_velocity_state_interface() { return has_velocity_state_interface_; } + bool has_velocity_state_interface() const { return has_velocity_state_interface_; } - bool has_acceleration_state_interface() { return has_acceleration_state_interface_; } + bool has_acceleration_state_interface() const { return has_acceleration_state_interface_; } - bool has_position_command_interface() { return has_position_command_interface_; } + bool has_position_command_interface() const { return has_position_command_interface_; } - bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_velocity_command_interface() const { return has_velocity_command_interface_; } - bool has_acceleration_command_interface() { return has_acceleration_command_interface_; } + bool has_acceleration_command_interface() const { return has_acceleration_command_interface_; } - bool has_effort_command_interface() { return has_effort_command_interface_; } + bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } - bool is_open_loop() { return params_.open_loop_control; } + bool is_open_loop() const { return params_.open_loop_control; } - bool has_active_traj() { return has_active_trajectory(); } + bool has_active_traj() const { return has_active_trajectory(); } - bool has_trivial_traj() + bool has_trivial_traj() const { return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; } From be366452e15f27bba73f7f44719c97ee1ddd1f56 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 16 Aug 2023 17:01:34 +0200 Subject: [PATCH 339/524] [JTC] Tolerance tests + Hold on time violation (#613) * Add new test to ensure that controller goes into position holding when tolerances are violated * Hold position if goal_time is exceeded with topic interface * Fix hold on time-violation --- .../src/joint_trajectory_controller.cpp | 14 ++++- .../test/test_trajectory_controller.cpp | 61 +++++++++++++++++++ 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9c5ee68132..92b3b5af8d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -362,19 +362,27 @@ controller_interface::return_type JointTrajectoryController::update( traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied or violated within the goal_time_tolerance } } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); } + else if ( + !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) + { + RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied (will stay in this state until new message arrives) + // or outside_goal_tolerance violated within the goal_time_tolerance } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 4149058c77..7850f15183 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1567,6 +1567,67 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, false, {params}, true); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectHoldingPoint(joint_state_pos_); +} + +TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) +{ + // set joint tolerance parameters + const double goal_tol = 0.1; + // set very small goal_time so that goal_time is violated + const double goal_time = 0.000001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, false, {params}, true); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectHoldingPoint(joint_state_pos_); +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, From bf9b7e802ca4005ca4a9927941b550f0853eab89 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 16 Aug 2023 17:54:35 +0100 Subject: [PATCH 340/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 19 files changed, 67 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index b4c1815bdc..377cdbd996 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 5f85999abd..622b740832 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- * Fix out of bound access in admittance controller (`#721 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index e100f537f6..7b0b59004c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 7fdcf063e2..60491ee06a 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.13.0 (2023-08-04) ------------------- * [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (`#533 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4b223619e0..de743ce39b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d9c0f2de24..04551f4d8a 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.13.0 (2023-08-04) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 324eead77a..538d974f6b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use tabs (`#743 `_) +* Contributors: Christoph Fröhlich + 3.13.0 (2023-08-04) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 506aaa48ec..2187613b1f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ef997c8733..3616493072 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.13.0 (2023-08-04) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9a5c4efc30..f4d6c40c2e 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.13.0 (2023-08-04) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index e2b1c6f613..283cdac64d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Tolerance tests + Hold on time violation (`#613 `_) + * Add new test to ensure that controller goes into position holding when tolerances are violated + * Hold position if goal_time is exceeded with topic interface + * Fix hold on time-violation +* [JTC] Fix typos, implicit cast, const member functions (`#748 `_) +* Remove wrong description (`#742 `_) +* [JTC] Explicitly set hold position (`#558 `_) +* Contributors: Christoph Fröhlich + 3.13.0 (2023-08-04) ------------------- * Small improvement in remapping (`#393 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 15242a2018..c9a11310e9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 04ed272d86..a1a91ccbb4 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d84a55f26a..08ce2bf191 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 3683d03c16..47735b98ce 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.13.0 (2023-08-04) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 5bf04ede76..094e3c77a6 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- * Update ci-ros-lint.yml and copyright format (`#720 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a40f09e557..f5d801cfe5 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 8d3c61ff6b..f4252616f5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b9fe5f9553..23875e9ec4 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.13.0 (2023-08-04) ------------------- From 102c1cf698acd80b10c00e19a901a82e066358d4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 16 Aug 2023 17:54:55 +0100 Subject: [PATCH 341/524] 3.14.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 +- 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 +- 40 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 377cdbd996..cad245cc4e 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index d4c6b3b3b4..1301b19bc9 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.13.0 + 3.14.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 622b740832..79c80f357e 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 966c268d93..4ea34b3852 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.13.0 + 3.14.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 7b0b59004c..0569a0fe8d 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 42693418f6..1b9844a090 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.13.0 + 3.14.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 60491ee06a..e12543aa85 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 80d3f6334e..df4d92a19b 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.13.0 + 3.14.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 de743ce39b..adc12b0caf 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 852cf77829..6edbd5454a 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.13.0 + 3.14.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 04551f4d8a..6ae71de6a6 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index b2c04e6a57..3907edbd31 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.13.0 + 3.14.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 538d974f6b..35ad79d4f2 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.14.0 (2023-08-16) +------------------- * Use tabs (`#743 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 25895a4696..522964019b 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.13.0 + 3.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 2187613b1f..28e73faf8c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index c977a0ed36..5804d3919b 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.13.0 + 3.14.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3616493072..9443ee51c2 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 41c087f3ca..8d26b48211 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.13.0 + 3.14.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 f4d6c40c2e..9ebf7177ba 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fd5deab3e7..919bac78f8 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.13.0 + 3.14.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 283cdac64d..88df65577c 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.14.0 (2023-08-16) +------------------- * [JTC] Tolerance tests + Hold on time violation (`#613 `_) * Add new test to ensure that controller goes into position holding when tolerances are violated * Hold position if goal_time is exceeded with topic interface diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 08a5392977..2941e11a56 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.13.0 + 3.14.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 c9a11310e9..7dcb2aadaa 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 1259239ac5..c88fc6ddc7 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.13.0 + 3.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a1a91ccbb4..b1758acc83 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 560ea5df88..ca4a031da6 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.13.0 + 3.14.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 08ce2bf191..3c92dbdb61 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 02af7f67b8..f5b887b01d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.13.0 + 3.14.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 6aceaa2cc2..2e1336e543 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.13.0", + version="3.14.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 47735b98ce..d931108078 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c2541f3a96..122b55116b 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.13.0 + 3.14.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 701c285e61..168d929a1c 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.13.0", + version="3.14.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 094e3c77a6..033fddb95a 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 1da3cfa9df..302b1cbd90 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.13.0 + 3.14.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 f5d801cfe5..df31f03c74 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9cc16d3b60..d8e1b0e6e8 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.13.0 + 3.14.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 f4252616f5..8fc50f7378 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.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 26083760d1..972300f590 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.13.0 + 3.14.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 23875e9ec4..49b945c3dc 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.14.0 (2023-08-16) +------------------- 3.13.0 (2023-08-04) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 3551021f82..e626c9849a 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.13.0 + 3.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 32aaef7552638826aba0b3f3a72b1c1453739afa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Fri, 18 Aug 2023 22:44:30 +0200 Subject: [PATCH 342/524] [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (#698) * Create ParamListener and get parameters on configure * Declare parameters for test_force_torque_sensor_broadcaster Since the parameters are not declared on init anymore, they cannot be set without declaring them before --------- Co-authored-by: Bence Magyar --- .../src/force_torque_sensor_broadcaster.cpp | 16 ++--- .../test_force_torque_sensor_broadcaster.cpp | 67 ++++++++++--------- 2 files changed, 44 insertions(+), 39 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9b570d353f..e9a173380b 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -29,6 +29,12 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() } controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { try { @@ -37,18 +43,10 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() } catch (const std::exception & e) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - const bool no_interface_names_defined = params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && 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 b88266712b..57650c9302 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 @@ -111,11 +111,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.z", "fts_sensor/torque.z"); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -126,7 +127,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", ""); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -137,8 +138,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", ""); + fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", ""); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -149,10 +150,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); // set the 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -163,11 +164,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.z", "fts_sensor/torque.z"); // set the 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -178,8 +180,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -191,8 +193,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -207,9 +209,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -223,8 +226,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -246,9 +249,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -270,13 +274,16 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.y", "fts_sensor/force.y"); + fts_broadcaster_->get_node()->declare_parameter("interface_names.force.z", "fts_sensor/force.z"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.x", "fts_sensor/torque.x"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.y", "fts_sensor/torque.y"); + fts_broadcaster_->get_node()->declare_parameter( + "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); From 1dac309979fb9392dc12feb6b46555582fed6fc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 21 Aug 2023 14:39:00 +0200 Subject: [PATCH 343/524] Update docs for diff drive controller (#751) --- admittance_controller/doc/userdoc.rst | 2 +- diff_drive_controller/doc/userdoc.rst | 61 ++++++++++++--------------- doc/writing_new_controller.rst | 2 +- 3 files changed, 30 insertions(+), 35 deletions(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 152ed890dc..0e4469cd50 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -11,7 +11,7 @@ The controller implements ``ChainedControllerInterface``, so it is possible to a The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. -ROS2 interface of the controller +ROS 2 interface of the controller --------------------------------- Parameters diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 6bd682af26..f8318bc4b8 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -6,71 +6,66 @@ diff_drive_controller ===================== Controller for mobile robots with differential drive. -Input for control are robot body velocity commands which are translated to wheel commands for the differential drive base. -Odometry is computed from hardware feedback and published. - -Velocity commands ------------------ -The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. +As input it takes velocity commands for the robot body, which are translated to wheel commands for the differential drive base. -Hardware interface type ------------------------ - -The controller works with wheel joints through a velocity interface. +Odometry is computed from hardware feedback and published. Other features -------------- - Realtime-safe implementation. - Odometry publishing - Task-space velocity, acceleration and jerk limits - Automatic stop after command time-out + + Realtime-safe implementation. + + Odometry publishing + + Task-space velocity, acceleration and jerk limits + + Automatic stop after command time-out -ros2_control Interfaces ------------------------- +Description of controller's interfaces +------------------------------------------------ References -,,,,,,,,,,, +,,,,,,,,,,,,,,,,,, +(the controller is not yet implemented as chainable controller) -States -,,,,,,, +Feedback +,,,,,,,,,,,,,, +As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``,if parameter ``position_feedback=false``) are used. -Commands +Output ,,,,,,,,, +Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. -ROS2 Interfaces ----------------- + +ROS 2 Interfaces +------------------------ Subscribers ,,,,,,,,,,,, + ~/cmd_vel [geometry_msgs/msg/TwistStamped] - Velocity command for the controller. + 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] - -~/cmd_vel_out [] - - + 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 ,,,,,,,,,,, -~/odom [] +~/odom [nav_msgs::msg::Odometry] + This represents an estimate of the robot's position and velocity in free space. -/tf +/tf [tf2_msgs::msg::TFMessage] + tf tree. Published only if ``enable_odom_tf=true`` - -Services -,,,,,,,,, +~/cmd_vel_out [geometry_msgs/msg/TwistStamped] + Velocity command for the controller, where limits were applied. Published only if ``publish_limited_velocity=true`` Parameters ------------- +,,,,,,,,,,,, Check `parameter definition file for details `_. diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 1543e79722..0475f8f4fd 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -27,7 +27,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 3. **Adding declarations into header file (.hpp)** - 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). + 1. Take care that you use header guards. ROS 2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). 2. include ``"controller_interface/controller_interface.hpp"`` and ``visibility_control.h`` if you are using one. From dcdbb94498260232038ccb7f1eee7373deddd39a Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Sep 2023 09:27:36 +0200 Subject: [PATCH 344/524] Bump actions/checkout from 3 to 4 (#763) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-format.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/humble-abi-compatibility.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/prerelease-check.yml | 2 +- .github/workflows/reusable-industrial-ci-with-cache.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- .github/workflows/reviewer_lottery.yml | 2 +- .github/workflows/rolling-abi-compatibility.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 11 files changed, 13 insertions(+), 13 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3d8a7431c0..cc5dd32596 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -19,7 +19,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/action-ros-ci@0.3.3 with: target-ros2-distro: ${{ env.ROS_DISTRO }} diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 8955904bea..c868650adb 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -11,7 +11,7 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4.7.0 with: python-version: '3.10' diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 3d102b53a2..bfb6fd3386 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -13,7 +13,7 @@ jobs: env: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: @@ -49,7 +49,7 @@ jobs: matrix: linter: [cpplint] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@master - uses: ros-tooling/action-ros-lint@master with: diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 3f38d5b6ce..708ea5c1f4 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 2a4b627d5e..9da6059892 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -22,7 +22,7 @@ jobs: ROS_DISTRO: humble container: ghcr.io/ros-controls/ros:humble-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_controllers - run: | diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 5e7326e510..856d877d85 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -24,7 +24,7 @@ jobs: pre_release: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ github.event.inputs.branch }} - name: industrial_ci diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index bca08ce5d2..acefeebfac 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -58,10 +58,10 @@ jobs: steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Checkout ${{ inputs.ref }} on scheduled build if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index d5f836f026..5a01d23533 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -29,7 +29,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - uses: ros-tooling/action-ros-ci@0.3.3 diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 772809825f..2edbc9b59e 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 4589e92e3b..3911434a23 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: rolling diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 91e645d9e3..04dc58775f 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -17,7 +17,7 @@ jobs: ROS_DISTRO: rolling container: ghcr.io/ros-controls/ros:rolling-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_controllers - run: | From 6352a70d210a6bdec6228a24f41b6d542d0e4364 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 7 Sep 2023 03:16:41 +0900 Subject: [PATCH 345/524] Fixed implementation so that effort_controllers/GripperActionController works. (#756) --- .../gripper_action_controller.hpp | 2 +- .../gripper_action_controller_impl.hpp | 23 ++++++++++--------- .../hardware_interface_adapter.hpp | 2 +- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 9d67249405..38e0bd8191 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -126,7 +126,7 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. std::optional> - joint_position_command_interface_; + joint_command_interface_; std::optional> joint_position_state_interface_; std::optional> diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 666fced1c0..77598591ae 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -226,20 +226,21 @@ template controller_interface::CallbackReturn GripperActionController::on_activate( const rclcpp_lifecycle::State &) { - auto position_command_interface_it = std::find_if( + auto command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_command_interface_it == command_interfaces_.end()) + { return command_interface.get_interface_name() == HardwareInterface; }); + if (command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Expected 1 " << HardwareInterface << " command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_prefix_name() != params_.joint) + if (command_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_prefix_name() << "` != `" + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -278,12 +279,12 @@ controller_interface::CallbackReturn GripperActionController: return controller_interface::CallbackReturn::ERROR; } - joint_position_command_interface_ = *position_command_interface_it; + joint_command_interface_ = *command_interface_it; joint_position_state_interface_ = *position_state_interface_it; joint_velocity_state_interface_ = *velocity_state_interface_it; // Hardware interface adapter - hw_iface_adapter_.init(joint_position_command_interface_, get_node()); + hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -311,7 +312,7 @@ template controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_position_command_interface_ = std::nullopt; + joint_command_interface_ = std::nullopt; joint_position_state_interface_ = std::nullopt; joint_velocity_state_interface_ = std::nullopt; release_interfaces(); @@ -324,7 +325,7 @@ GripperActionController::command_interface_configuration() co { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + hardware_interface::HW_IF_POSITION}}; + {params_.joint + "/" + HardwareInterface}}; } template diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 97ebbb1f4b..b125ab12d0 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -131,7 +131,7 @@ class HardwareInterfaceAdapter { joint_handle_ = joint_handle; // Init PID gains from ROS parameter server - const std::string prefix = "gains." + joint_handle_->get().get_name(); + const std::string prefix = "gains." + joint_handle_->get().get_prefix_name(); const auto k_p = auto_declare(node, prefix + ".p", 0.0); const auto k_i = auto_declare(node, prefix + ".i", 0.0); const auto k_d = auto_declare(node, prefix + ".d", 0.0); From 5b86e3c848a0f1498978e24fe5b1b48566c2d14e Mon Sep 17 00:00:00 2001 From: flochre <39066045+flochre@users.noreply.github.com> Date: Thu, 7 Sep 2023 06:17:30 +0600 Subject: [PATCH 346/524] add a broadcaster for range sensor (#725) --- range_sensor_broadcaster/CMakeLists.txt | 106 ++++++++ range_sensor_broadcaster/README.md | 8 + range_sensor_broadcaster/doc/userdoc.rst | 15 ++ .../range_sensor_broadcaster.hpp | 77 ++++++ .../visibility_control.h | 53 ++++ range_sensor_broadcaster/package.xml | 31 +++ .../range_sensor_broadcaster.xml | 8 + .../src/range_sensor_broadcaster.cpp | 138 ++++++++++ .../range_sensor_broadcaster_parameters.yaml | 16 ++ .../test/range_sensor_broadcaster_params.yaml | 12 + .../test_load_range_sensor_broadcaster.cpp | 52 ++++ .../test/test_range_sensor_broadcaster.cpp | 252 ++++++++++++++++++ .../test/test_range_sensor_broadcaster.hpp | 59 ++++ ros2_controllers/package.xml | 1 + 14 files changed, 828 insertions(+) create mode 100644 range_sensor_broadcaster/CMakeLists.txt create mode 100644 range_sensor_broadcaster/README.md create mode 100644 range_sensor_broadcaster/doc/userdoc.rst create mode 100644 range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp create mode 100644 range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h create mode 100644 range_sensor_broadcaster/package.xml create mode 100644 range_sensor_broadcaster/range_sensor_broadcaster.xml create mode 100644 range_sensor_broadcaster/src/range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml create mode 100644 range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml create mode 100644 range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp create mode 100644 range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..00da395db5 --- /dev/null +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.8) +project(range_sensor_broadcaster) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +generate_parameter_library(range_sensor_broadcaster_parameters + src/range_sensor_broadcaster_parameters.yaml +) + +add_library( + range_sensor_broadcaster SHARED + src/range_sensor_broadcaster.cpp +) +target_include_directories(range_sensor_broadcaster + PRIVATE $ + PRIVATE $ +) +ament_target_dependencies(range_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(range_sensor_broadcaster PRIVATE "RANGE_SENSOR_BROADCASTER_BUILDING_DLL") +target_link_libraries(range_sensor_broadcaster + range_sensor_broadcaster_parameters +) + +pluginlib_export_plugin_description_file( + controller_interface range_sensor_broadcaster.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster + test/test_load_range_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + target_include_directories(test_load_range_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_range_sensor_broadcaster + range_sensor_broadcaster + ) + ament_target_dependencies(test_load_range_sensor_broadcaster + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_range_sensor_broadcaster + test/test_range_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + target_include_directories(test_range_sensor_broadcaster PRIVATE include) + target_link_libraries(test_range_sensor_broadcaster + range_sensor_broadcaster + ) + ament_target_dependencies(test_range_sensor_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + range_sensor_broadcaster + range_sensor_broadcaster_parameters + EXPORT export_range_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_range_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/range_sensor_broadcaster/README.md b/range_sensor_broadcaster/README.md new file mode 100644 index 0000000000..33195d272b --- /dev/null +++ b/range_sensor_broadcaster/README.md @@ -0,0 +1,8 @@ +range_sensor_broadcaster +========================================== + +Controller to publish readings of Range sensors. + +Pluginlib-Library: range_sensor_broadcaster + +Plugin: range_sensor_broadcaster/RangeSensorBroadcaster (controller_interface::ControllerInterface) diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..e35bec67ad --- /dev/null +++ b/range_sensor_broadcaster/doc/userdoc.rst @@ -0,0 +1,15 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/range_sensor_broadcaster/doc/userdoc.rst + +.. _range_sensor_broadcaster_userdoc: + +Range Sensor Broadcaster +-------------------------------- +Broadcaster of messages from Range sensor. +The published message type is ``sensor_msgs/msg/Range``. + +The controller is a wrapper around ``RangeSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ + +.. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..b2e5fbfac0 --- /dev/null +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -0,0 +1,77 @@ +// Copyright 2021 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Subhas Das, Denis Stogl, Victor Lopez + */ + +#ifndef RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ +#define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "range_sensor_broadcaster/visibility_control.h" +#include "range_sensor_broadcaster_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/range_sensor.hpp" +#include "sensor_msgs/msg/range.hpp" + +namespace range_sensor_broadcaster +{ +class RangeSensorBroadcaster : public controller_interface::ControllerInterface +{ +public: + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + RANGE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr range_sensor_; + + using StatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; +}; + +} // namespace range_sensor_broadcaster + +#endif // RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h new file mode 100644 index 0000000000..0a9a9f53a8 --- /dev/null +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Subhas Das, Denis Stogl + */ + +#ifndef RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ +#define RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport)) +#define RANGE_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define RANGE_SENSOR_BROADCASTER_EXPORT __declspec(dllexport) +#define RANGE_SENSOR_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef RANGE_SENSOR_BROADCASTER_BUILDING_DLL +#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_EXPORT +#else +#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_IMPORT +#endif +#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE RANGE_SENSOR_BROADCASTER_PUBLIC +#define RANGE_SENSOR_BROADCASTER_LOCAL +#else +#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define RANGE_SENSOR_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define RANGE_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define RANGE_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define RANGE_SENSOR_BROADCASTER_PUBLIC +#define RANGE_SENSOR_BROADCASTER_LOCAL +#endif +#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml new file mode 100644 index 0000000000..11175d9f03 --- /dev/null +++ b/range_sensor_broadcaster/package.xml @@ -0,0 +1,31 @@ + + + + range_sensor_broadcaster + 3.14.0 + Controller to publish readings of Range sensors. + Bence Magyar + Florent Chretien + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/range_sensor_broadcaster/range_sensor_broadcaster.xml b/range_sensor_broadcaster/range_sensor_broadcaster.xml new file mode 100644 index 0000000000..fd82c7ae25 --- /dev/null +++ b/range_sensor_broadcaster/range_sensor_broadcaster.xml @@ -0,0 +1,8 @@ + + + + This controller publishes the readings of an Range sensor as sensor_msgs/Range message. + + + diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..b821da8c13 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -0,0 +1,138 @@ +// Copyright 2021 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: Subhas Das, Denis Stogl, Victor Lopez + */ + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +#include +#include + +namespace range_sensor_broadcaster +{ +controller_interface::CallbackReturn RangeSensorBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + if (params_.sensor_name.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); + return CallbackReturn::ERROR; + } + + if (params_.frame_id.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); + return CallbackReturn::ERROR; + } + + range_sensor_ = std::make_unique( + semantic_components::RangeSensor(params_.sensor_name)); + try + { + // register ft sensor data publisher + sensor_state_publisher_ = + get_node()->create_publisher("~/range", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->msg_.radiation_type = params_.radiation_type; + realtime_publisher_->msg_.field_of_view = params_.field_of_view; + realtime_publisher_->msg_.min_range = params_.min_range; + realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->msg_.variance = params_.variance; + realtime_publisher_->unlock(); + + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +RangeSensorBroadcaster::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration RangeSensorBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = range_sensor_->get_state_interface_names(); + return state_interfaces_config; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + range_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type RangeSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + range_sensor_->get_values_as_message(realtime_publisher_->msg_); + realtime_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace range_sensor_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + range_sensor_broadcaster::RangeSensorBroadcaster, controller_interface::ControllerInterface) diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..57d39d20c4 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml @@ -0,0 +1,16 @@ +range_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } + radiation_type: {type: int, default_value: 0, description: "The type of radiation used by the sensor / 0 = Ultrason / 1 = Infrared",} + field_of_view: {type: double, default_value: 0.52, description: "The size of the arc that the distance reading is valid for [rad]",} + min_range: {type: double, default_value: 0.52, description: "Minimum range value [m]",} + max_range: {type: double, default_value: 4.0, description: "Maximum range value [m]",} + variance: {type: double, default_value: 0.0, description: "Variance of the range value",} diff --git a/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..1e59a7f27b --- /dev/null +++ b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml @@ -0,0 +1,12 @@ +test_range_sensor_broadcaster: + ros__parameters: + # Setting mendatory parameters + sensor_name: "range_sensor" + frame_id: "range_sensor_frame" + + # Setting parameters with changed default value to check those are used + radiation_type: 1 + field_of_view: 0.1 + min_range: 0.10 + max_range: 7.0 + variance: 1.0 diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..5c400bef91 --- /dev/null +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Subhas Das, Denis Stogl, Florent Chretien + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadRangeSensorBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp new file mode 100644 index 0000000000..855f7e037b --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -0,0 +1,252 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: flochre + */ + +#include + +#include "test_range_sensor_broadcaster.hpp" + +#include "hardware_interface/loaned_state_interface.hpp" + +void RangeSensorBroadcasterTest::SetUp() +{ + // initialize controller + range_broadcaster_ = std::make_unique(); +} + +void RangeSensorBroadcasterTest::TearDown() { range_broadcaster_.reset(nullptr); } + +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); + + if (controller_interface::return_type::OK == result) + { + std::vector state_interfaces; + state_interfaces.emplace_back(range_); + + range_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); + } + + return result; +} + +controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broadcaster( + std::vector & parameters) +{ + // Configure the broadcaster + for (auto parameter : parameters) + { + range_broadcaster_->get_node()->set_parameter(parameter); + } + + return range_broadcaster_->on_configure(rclcpp_lifecycle::State()); +} + +void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) +{ + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_range_sensor_broadcaster/range", 10, subs_callback); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(range_msg, msg_info)); +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) +{ + ASSERT_THROW(init_broadcaster(""), std::exception); +} + +TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Success) +{ + ASSERT_EQ( + init_broadcaster("test_range_sensor_broadcaster"), controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_1) +{ + // First Test without frame_id ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty sensor name to generate an error + parameters.emplace_back(rclcpp::Parameter("sensor_name", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_2) +{ + // Second Test without sensor_name ERROR Expected + init_broadcaster("test_range_sensor_broadcaster"); + + std::vector parameters; + // explicitly give an empty frame_id to generate an error + parameters.emplace_back(rclcpp::Parameter("frame_id", "")); + ASSERT_EQ(configure_broadcaster(parameters), controller_interface::CallbackReturn::ERROR); +} + +TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) +{ + // Third Test without sensor_name SUCCESS Expected + init_broadcaster("test_range_sensor_broadcaster"); + + ASSERT_EQ( + range_broadcaster_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + ASSERT_EQ( + range_broadcaster_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + auto result = range_broadcaster_->update( + range_broadcaster_->get_node()->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.10; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + + sensor_range_ = 4.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success) +{ + init_broadcaster("test_range_sensor_broadcaster"); + + range_broadcaster_->on_configure(rclcpp_lifecycle::State()); + range_broadcaster_->on_activate(rclcpp_lifecycle::State()); + + sensor_msgs::msg::Range range_msg; + + sensor_range_ = 0.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + + sensor_range_ = 6.0; + subscribe_and_get_message(range_msg); + + EXPECT_EQ(range_msg.header.frame_id, frame_id_); + // Even out of boundaries you will get the out_of_range range value + EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_EQ(range_msg.radiation_type, radiation_type_); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp new file mode 100644 index 0000000000..10696d071f --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 flochre +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Authors: flochre + */ + +#ifndef TEST_RANGE_SENSOR_BROADCASTER_HPP_ +#define TEST_RANGE_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" + +class RangeSensorBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + // for the sake of the test + // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml + const std::string sensor_name_ = "range_sensor"; + const std::string frame_id_ = "range_sensor_frame"; + + const double field_of_view_ = 0.1; + const int radiation_type_ = 1; + const double min_range_ = 0.1; + const double max_range_ = 7.0; + const double variance_ = 1.0; + + double sensor_range_ = 3.1; + hardware_interface::StateInterface range_{sensor_name_, "range", &sensor_range_}; + + std::unique_ptr range_broadcaster_; + + controller_interface::return_type init_broadcaster(std::string broadcaster_name); + controller_interface::CallbackReturn configure_broadcaster( + std::vector & parameters); + void subscribe_and_get_message(sensor_msgs::msg::Range & range_msg); +}; + +#endif // TEST_RANGE_SENSOR_BROADCASTER_HPP_ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ca4a031da6..51d4d9ae44 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -21,6 +21,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + range_sensor_broadcaster steering_controllers_library tricycle_controller tricycle_steering_controller From 4d0b999540177f4237550184caae0f2960d5b72b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 7 Sep 2023 19:00:45 +0200 Subject: [PATCH 347/524] [Docs+CI] Add range sensor to controller index and lint job (#767) --- .github/workflows/ci-ros-lint.yml | 2 ++ doc/controllers_index.rst | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index bfb6fd3386..5789c2dee4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -32,6 +32,7 @@ jobs: joint_state_broadcaster joint_trajectory_controller position_controllers + range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes rqt_joint_trajectory_controller @@ -69,6 +70,7 @@ jobs: joint_state_broadcaster joint_trajectory_controller position_controllers + range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes rqt_joint_trajectory_controller diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 763381c065..de33c3284c 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -63,6 +63,7 @@ Available Broadcasters .. toctree:: :titlesonly: - Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> - Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> + Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> + Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> From fb46a77df7a6179a7724c7ac0145b0be4740cbfa Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 8 Sep 2023 16:30:34 +0900 Subject: [PATCH 348/524] Add test for effort gripper controller (#769) --- .../test/test_gripper_controllers.cpp | 103 +++++++++++------- .../test/test_gripper_controllers.hpp | 10 +- 2 files changed, 67 insertions(+), 46 deletions(-) diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 4c82eb6961..506f968b99 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -32,123 +32,142 @@ using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +template +void GripperControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +template +void GripperControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} -void GripperControllerTest::SetUp() +template +void GripperControllerTest::SetUp() { // initialize controller - controller_ = std::make_unique(); + controller_ = std::make_unique>(); } -void GripperControllerTest::TearDown() { controller_.reset(nullptr); } +template +void GripperControllerTest::TearDown() +{ + controller_.reset(nullptr); +} -void GripperControllerTest::SetUpController() +template +void GripperControllerTest::SetUpController() { const auto result = controller_->init("gripper_controller"); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } -TEST_F(GripperControllerTest, ParametersNotSet) +using TestTypes = ::testing::Types< + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(GripperControllerTest, TestTypes); + +TYPED_TEST(GripperControllerTest, ParametersNotSet) { - SetUpController(); + this->SetUpController(); // configure failed, 'joints' parameter not set ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, JointParameterIsEmpty) +TYPED_TEST(GripperControllerTest, JointParameterIsEmpty) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", ""}); + this->controller_->get_node()->set_parameter({"joint", ""}); // configure failed, 'joints' is empty ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ConfigureParamsSuccess) +TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); // configure successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); // activate successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } -TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateDeactivateActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_deactivate(rclcpp_lifecycle::State()), + this->controller_->on_deactivate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); // re-assign interfaces std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 350e551cb8..4983c8102d 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -26,20 +26,22 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::StateInterface; namespace { - // subclassing and friending so we can access member variables +template class FriendGripperController -: public gripper_action_controller::GripperActionController +: public gripper_action_controller::GripperActionController { FRIEND_TEST(GripperControllerTest, CommandSuccessTest); }; +template class GripperControllerTest : public ::testing::Test { public: @@ -53,7 +55,7 @@ class GripperControllerTest : public ::testing::Test void SetUpHandles(); protected: - std::unique_ptr controller_; + std::unique_ptr> controller_; // dummy joint state values used for tests const std::string joint_name_ = "joint1"; @@ -62,7 +64,7 @@ class GripperControllerTest : public ::testing::Test StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]}; }; } // anonymous namespace From 4e917db2474ae84573165fc0414ecaccbf8698b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Sep 2023 11:28:37 +0200 Subject: [PATCH 349/524] [JTC] Make most parameters read-only (#771) --- joint_trajectory_controller/CMakeLists.txt | 3 +- .../src/joint_trajectory_controller.cpp | 2 +- ...oint_trajectory_controller_parameters.yaml | 7 +++ .../test_joint_trajectory_controller.yaml | 12 ----- .../test/test_trajectory_actions.cpp | 3 +- .../test/test_trajectory_controller.cpp | 49 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 35 ++++++------- 7 files changed, 47 insertions(+), 64 deletions(-) delete mode 100644 joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 0552e0bc86..88bb7891a3 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -60,8 +60,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) ament_add_gmock(test_trajectory_controller - test/test_trajectory_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) + test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_link_libraries(test_trajectory_controller joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 92b3b5af8d..7e1cd04a15 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -951,7 +951,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } - // Should the controller start by holding position after on_configure? + // Should the controller start by holding position at the beginning of active state? if (params_.start_with_holding) { add_new_trajectory_msg(set_hold_position()); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 5b8a8d14f2..0daf3f41bd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -3,6 +3,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -11,6 +12,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of the commanding joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -19,6 +21,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of command interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -29,6 +32,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of state interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], @@ -44,6 +48,7 @@ joint_trajectory_controller: type: bool, default_value: false, description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + read_only: true, } allow_integration_in_goal_trajectories: { type: bool, @@ -54,6 +59,7 @@ joint_trajectory_controller: type: double, default_value: 20.0, description: "Rate status changes will be monitored", + read_only: true, validation: { gt_eq: [0.1] } @@ -62,6 +68,7 @@ joint_trajectory_controller: type: string, default_value: "splines", description: "The type of interpolation to use, if any", + read_only: true, validation: { one_of<>: [["splines", "none"]], } diff --git a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml deleted file mode 100644 index 09a134e06a..0000000000 --- a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml +++ /dev/null @@ -1,12 +0,0 @@ -test_joint_trajectory_controller: - joints: - - joint1 - - joint2 - - joint3 - - command_interfaces: - - position - - state_interfaces: - - position - - velocity diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 2c28319d52..fc24c2c029 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -72,8 +72,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest { setup_executor_ = true; - SetUpAndActivateTrajectoryController( - executor_, true, parameters, separate_cmd_and_state_values); + SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values); SetUpActionClient(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7850f15183..e0655d2368 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -138,11 +138,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor); - // set command_joints parameter const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); - traj_controller_->get_node()->set_parameter({command_joint_names_param}); + SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -206,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) rclcpp::executors::MultiThreadedExecutor executor; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params); + SetUpAndActivateTrajectoryController(executor, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -252,12 +250,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor, false); + SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - SetParameters(); traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -324,7 +321,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); updateController(); @@ -426,7 +423,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateController(rclcpp::Duration(FIRST_POINT_TIME)); @@ -444,7 +441,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateController(rclcpp::Duration(FIRST_POINT_TIME)); @@ -468,7 +465,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) constexpr double k_p = 10.0; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false); + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -577,7 +574,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) constexpr double k_p = 10.0; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true); + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -720,7 +717,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}, true); + SetUpAndActivateTrajectoryController(executor, {}, true); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -846,7 +843,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; @@ -918,7 +915,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; @@ -1000,7 +997,7 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController( - executor, true, {partial_joints_parameters, allow_integration_parameters}); + executor, {partial_joints_parameters, allow_integration_parameters}); trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; @@ -1061,7 +1058,7 @@ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {allow_integration_parameters}); + SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; @@ -1123,7 +1120,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) { rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); subscribeToState(); @@ -1166,7 +1163,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations @@ -1196,7 +1193,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1229,7 +1226,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur { rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); subscribeToState(); RCLCPP_WARN( @@ -1289,7 +1286,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1390,7 +1387,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e { rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1488,7 +1485,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co joint_vel_[i] = std::numeric_limits::quiet_NaN(); joint_acc_[i] = std::numeric_limits::quiet_NaN(); } - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1535,7 +1532,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co joint_vel_[i] = 0.25 + static_cast(i); joint_acc_[i] = 0.02 + static_cast(i) / 10.0; } - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1577,7 +1574,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, false, {params}, true); + SetUpAndActivateTrajectoryController(executor, params, true); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1609,7 +1606,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.goal_time", goal_time)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, false, {params}, true); + SetUpAndActivateTrajectoryController(executor, params, true); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8a22f986aa..59d20c33d2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -162,32 +162,26 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); } - void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) + void SetUpTrajectoryController( + rclcpp::Executor & executor, const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); - if (use_local_parameters) - { - traj_controller_->set_joint_names(joint_names_); - traj_controller_->set_command_interfaces(command_interface_types_); - traj_controller_->set_state_interfaces(state_interface_types_); - } - auto ret = traj_controller_->init(controller_name_); + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_)); + parameter_overrides.push_back( + rclcpp::Parameter("command_interfaces", command_interface_types_)); + parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); + 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); if (ret != controller_interface::return_type::OK) { FAIL(); } executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - SetParameters(); - } - - void SetParameters() - { - auto node = traj_controller_->get_node(); - const rclcpp::Parameter joint_names_param("joints", joint_names_); - const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); - const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); - node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } void SetPidParameters( @@ -210,12 +204,11 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpAndActivateTrajectoryController( - rclcpp::Executor & executor, bool use_local_parameters = true, - const std::vector & parameters = {}, + 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) { - SetUpTrajectoryController(executor, use_local_parameters); + SetUpTrajectoryController(executor); // set pid parameters before configure SetPidParameters(k_p, ff, normalize_error); From be3b6c1867516b892a3a5e3a076fd330379fce8f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 11 Sep 2023 14:46:16 +0100 Subject: [PATCH 350/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 5 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 5 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 6 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 5 + position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 149 ++++++++++++++++++ ros2_controllers/CHANGELOG.rst | 5 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 20 files changed, 219 insertions(+) create mode 100644 range_sensor_broadcaster/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index cad245cc4e..6aec45a94b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 79c80f357e..0bfc81e20d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update docs for diff drive controller (`#751 `_) +* Contributors: Christoph Fröhlich + 3.14.0 (2023-08-16) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 0569a0fe8d..7798499535 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e12543aa85..e1e9ef0f14 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update docs for diff drive controller (`#751 `_) +* Contributors: Christoph Fröhlich + 3.14.0 (2023-08-16) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index adc12b0caf..2b57ffdaed 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 6ae71de6a6..261cdd2406 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_) +* Contributors: Noel Jiménez García + 3.14.0 (2023-08-16) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 35ad79d4f2..e7adf8759a 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.14.0 (2023-08-16) ------------------- * Use tabs (`#743 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 28e73faf8c..2535205c0e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test for effort gripper controller (`#769 `_) +* Fixed implementation so that effort_controllers/GripperActionController works. (`#756 `_) +* Contributors: chama1176 + 3.14.0 (2023-08-16) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9443ee51c2..eee2391f3c 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.14.0 (2023-08-16) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9ebf7177ba..9d277af9d6 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.14.0 (2023-08-16) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 88df65577c..9e55d425b6 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Make most parameters read-only (`#771 `_) +* Contributors: Christoph Fröhlich + 3.14.0 (2023-08-16) ------------------- * [JTC] Tolerance tests + Hold on time violation (`#613 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7dcb2aadaa..92e530b5dc 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..76e55d05f0 --- /dev/null +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package range_sensor_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* add a broadcaster for range sensor (`#725 `_) +* Contributors: flochre + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b1758acc83..2f35c7568b 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add a broadcaster for range sensor (`#725 `_) +* Contributors: flochre + 3.14.0 (2023-08-16) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 3c92dbdb61..742a89fb40 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d931108078..1ebf3a133a 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.14.0 (2023-08-16) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 033fddb95a..44273d2561 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index df31f03c74..b64fdaedbc 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 8fc50f7378..7431aa9160 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 49b945c3dc..5bc0a93bd2 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.14.0 (2023-08-16) ------------------- From ab1f935236565f27a08e9579196523d770d72173 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 11 Sep 2023 14:46:45 +0100 Subject: [PATCH 351/524] 3.15.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- 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 6aec45a94b..723ca88779 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 1301b19bc9..40a3545b99 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.14.0 + 3.15.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0bfc81e20d..593fa56562 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- * Update docs for diff drive controller (`#751 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 4ea34b3852..9f2de80dad 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.14.0 + 3.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 7798499535..be6ac52313 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 1b9844a090..ddc110f81a 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.14.0 + 3.15.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e1e9ef0f14..52c8572739 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.15.0 (2023-09-11) +------------------- * Update docs for diff drive controller (`#751 `_) * Contributors: Christoph Fröhlich diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index df4d92a19b..cc849d12ef 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.14.0 + 3.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 2b57ffdaed..4b93f1f69a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6edbd5454a..d0aca9701a 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.14.0 + 3.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 261cdd2406..296cad54bf 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.15.0 (2023-09-11) +------------------- * [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_) * Contributors: Noel Jiménez García diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 3907edbd31..4cbad1c014 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.14.0 + 3.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e7adf8759a..b3d33964b6 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 522964019b..af90230c7e 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.14.0 + 3.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 2535205c0e..82cf6cf45b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- * Add test for effort gripper controller (`#769 `_) * Fixed implementation so that effort_controllers/GripperActionController works. (`#756 `_) * Contributors: chama1176 diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 5804d3919b..d3c8d9bf9f 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.14.0 + 3.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index eee2391f3c..915544197c 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 8d26b48211..31684ec958 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.14.0 + 3.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9d277af9d6..57bc1b7f45 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 919bac78f8..248dbcb099 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.14.0 + 3.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9e55d425b6..8b1a1c6a54 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.15.0 (2023-09-11) +------------------- * [JTC] Make most parameters read-only (`#771 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 2941e11a56..521aa0ea9d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.14.0 + 3.15.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 92e530b5dc..5da7a35e1a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c88fc6ddc7..c947f00bdb 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.14.0 + 3.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 76e55d05f0..6e63a22917 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.15.0 (2023-09-11) +------------------- * add a broadcaster for range sensor (`#725 `_) * Contributors: flochre diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 11175d9f03..33ed7d5e4d 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.14.0 + 3.15.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2f35c7568b..6684adfabc 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- * add a broadcaster for range sensor (`#725 `_) * Contributors: flochre diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 51d4d9ae44..ea2456486b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.14.0 + 3.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 742a89fb40..3caadb3880 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f5b887b01d..845e65d233 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.14.0 + 3.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 2e1336e543..e4d9a72f89 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.14.0", + version="3.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 1ebf3a133a..180dd35033 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 122b55116b..9987061405 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.14.0 + 3.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 168d929a1c..0dea799d80 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.14.0", + version="3.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44273d2561..4c4dae365d 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 302b1cbd90..48c137460b 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.14.0 + 3.15.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index b64fdaedbc..625c0f4a06 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index d8e1b0e6e8..a53be880f2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.14.0 + 3.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 7431aa9160..afe6360322 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.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 972300f590..f320c08c9f 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.14.0 + 3.15.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 5bc0a93bd2..2551b76bd8 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.15.0 (2023-09-11) +------------------- 3.14.0 (2023-08-16) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e626c9849a..d0b6cf0ed1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.14.0 + 3.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 923767fcf8df2ef82c313a5cfadb1e0fbe46cab2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Sep 2023 17:40:15 +0200 Subject: [PATCH 352/524] [JTC] Add note on goal_time=0 in docs (#773) --- joint_trajectory_controller/doc/parameters.rst | 1 + .../src/joint_trajectory_controller_parameters.yaml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index d5ddb831ca..199f1b30dc 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -78,6 +78,7 @@ constraints.stopped_velocity_tolerance (double) constraints.goal_time (double) Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + If set to zero, the controller will wait a potentially infinite amount of time. Default: 0.0 (not checked) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 0daf3f41bd..d299944976 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -124,7 +124,8 @@ joint_trajectory_controller: goal_time: { type: double, default_value: 0.0, - description: "Time tolerance for achieving trajectory goal before or after commanded time.", + description: "Time tolerance for achieving trajectory goal before or after commanded time. + If set to zero, the controller will wait a potentially infinite amount of time.", validation: { gt_eq: [0.0], } From 66c5ac384bc3eac0ec5fb41fe92cdae9b3c5ac9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Sep 2023 11:54:55 +0200 Subject: [PATCH 353/524] [JTC] Fix hold position mode with goal_time>0 (#758) --- .../joint_trajectory_controller.hpp | 4 + .../src/joint_trajectory_controller.cpp | 64 +++++++++------ .../test/test_trajectory_actions.cpp | 24 +++++- .../test/test_trajectory_controller.cpp | 12 ++- .../test/test_trajectory_controller_utils.hpp | 81 ++++++++++++++----- 5 files changed, 132 insertions(+), 53 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 1df557d029..1f805ee95b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -174,6 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; + realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -187,6 +188,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; + std::shared_ptr hold_position_msg_ptr_ = nullptr; + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; @@ -270,6 +273,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); + void init_hold_position_msg(); void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7e1cd04a15..229456c104 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -227,7 +227,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, default_tolerances_.state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { tolerance_violated_while_moving = true; } @@ -235,7 +236,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { outside_goal_tolerance = true; @@ -805,6 +807,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( logger, "Using '%s' interpolation method.", interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + // prepare hold_position_msg + init_hold_position_msg(); + + // create subscriber and publishers joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), @@ -956,6 +962,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { add_new_trajectory_msg(set_hold_position()); } + rt_is_holding_.writeFromNonRT(true); return CallbackReturn::SUCCESS; } @@ -1100,6 +1107,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); } }; @@ -1162,6 +1170,7 @@ void JointTrajectoryController::goal_accepted_callback( std::make_shared(goal_handle->get_goal()->trajectory); add_new_trajectory_msg(traj_msg); + rt_is_holding_.writeFromNonRT(false); } // Update the active goal @@ -1456,31 +1465,12 @@ std::shared_ptr JointTrajectoryController::set_hold_position() { // Command to stay at current position - trajectory_msgs::msg::JointTrajectory current_pose_msg; - current_pose_msg.header.stamp = - rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately - current_pose_msg.joint_names = params_.joints; - current_pose_msg.points.push_back(state_current_); - current_pose_msg.points[0].velocities.clear(); - current_pose_msg.points[0].accelerations.clear(); - current_pose_msg.points[0].effort.clear(); - if (has_velocity_command_interface_) - { - // ensure no velocity (PID will fix this) - current_pose_msg.points[0].velocities.resize(dof_, 0.0); - } - if (has_acceleration_command_interface_) - { - // ensure no acceleration - current_pose_msg.points[0].accelerations.resize(dof_, 0.0); - } - if (has_effort_command_interface_) - { - // ensure no explicit effort (PID will fix this) - current_pose_msg.points[0].effort.resize(dof_, 0.0); - } + hold_position_msg_ptr_->points[0].positions = state_current_.positions; - return std::make_shared(current_pose_msg); + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; } bool JointTrajectoryController::contains_interface_type( @@ -1530,6 +1520,28 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::init_hold_position_msg() +{ + hold_position_msg_ptr_ = std::make_shared(); + hold_position_msg_ptr_->header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + hold_position_msg_ptr_->joint_names = params_.joints; + hold_position_msg_ptr_->points.resize(1); // a trivial msg only + hold_position_msg_ptr_->points[0].velocities.clear(); + hold_position_msg_ptr_->points[0].accelerations.clear(); + hold_position_msg_ptr_->points[0].effort.clear(); + if (has_velocity_command_interface_ || has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns velocity points in any case + hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns acceleration points in any case + hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fc24c2c029..371be914e2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double kp = 0.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values); + SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); SetUpActionClient(); @@ -247,7 +247,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - expectHoldingPoint(point_positions); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(point_positions); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) @@ -294,7 +302,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points_positions.at(1)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } /** diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e0655d2368..e02215e842 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1574,7 +1574,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, params, true, kp); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1606,7 +1607,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.goal_time", goal_time)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + SetUpAndActivateTrajectoryController(executor, params, true, 1.0); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1623,6 +1624,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); + + // what happens if we wait longer but it harms the tolerance again? + auto hold_position = joint_state_pos_; + joint_state_pos_.at(0) = -3.3; + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // it should be still holding the old point + expectHoldingPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 59d20c33d2..f25f2a03f8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -38,6 +38,12 @@ const std::vector INITIAL_POS_JOINTS = { const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; + +bool is_same_sign_or_zero(double val1, double val2) +{ + return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); +} + } // namespace namespace test_trajectory_controllers @@ -435,32 +441,65 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->has_position_command_interface()) + if (traj_controller_->use_closed_loop_pid_adapter() == false) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); - } + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); - } + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } - if (traj_controller_->has_acceleration_command_interface()) - { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } - if (traj_controller_->has_effort_command_interface()) + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + else { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + // velocity or effort PID? + // velocity setpoint is always zero -> feedforward term does not have an effect + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - 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]; + } + 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]; + } } } From cad7894f64ea26fd5c1fe9aa29f067bb5b0304e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 13 Sep 2023 11:12:03 +0200 Subject: [PATCH 354/524] [JTC] Rename parameter: normalize_error to angle_wraparound (#772) --- joint_trajectory_controller/doc/parameters.rst | 10 ++++++---- .../src/joint_trajectory_controller.cpp | 13 +++++++++++-- .../src/joint_trajectory_controller_parameters.yaml | 8 +++++++- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 199f1b30dc..bb16f5b250 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -102,7 +102,7 @@ gains (structure) u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. gains..p (double) @@ -130,10 +130,12 @@ gains..ff_velocity_scale (double) Default: 0.0 -gains..normalize_error (bool) +gains..angle_wraparound (bool) + For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. 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. Use this for revolute joints without end stop, - where the shortest rotation to the target position is the desired motion. + position :math:`s` from the state interface. + Default: false diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 229456c104..ad40fc2da8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -726,12 +726,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } - // Configure joint position error normalization from ROS parameters + // Configure joint position error normalization from ROS parameters (angle_wraparound) normalize_joint_error_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - normalize_joint_error_[i] = gains.normalize_error; + if (gains.normalize_error) + { + // TODO(anyone): Remove deprecation warning in the end of 2023 + RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!"); + normalize_joint_error_[i] = gains.normalize_error; + } + else + { + normalize_joint_error_[i] = gains.angle_wraparound; + } } if (params_.state_interfaces.empty()) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index d299944976..5a1d985a7a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -113,7 +113,13 @@ joint_trajectory_controller: normalize_error: { type: bool, default_value: false, - description: "Use position error normalization to -pi to pi." + description: "(Deprecated) Use position error normalization to -pi to pi." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." } constraints: stopped_velocity_tolerance: { From 0f08731a7ca236dd129321f7c81eb10cfaffa20e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 14 Sep 2023 18:20:07 +0200 Subject: [PATCH 355/524] [JTC] Add time-out for trajectory interfaces (#609) --- .../doc/parameters.rst | 10 ++ .../joint_trajectory_controller.hpp | 5 +- .../trajectory.hpp | 3 - .../src/joint_trajectory_controller.cpp | 50 ++++++- ...oint_trajectory_controller_parameters.yaml | 8 + .../test/test_trajectory_controller.cpp | 141 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 7 + 7 files changed, 212 insertions(+), 12 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index bb16f5b250..95a6599191 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -68,6 +68,16 @@ allow_nonzero_velocity_at_trajectory_end (boolean) Default: true +cmd_timeout (double) + Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + ``cmd_timeout`` must be greater than ``constraints.goal_time``, + otherwise ignored. + + If zero, timeout is deactivated" + + Default: 0.0 + constraints (structure) Default values for tolerances if no explicit values are states in JointTrajectory message. 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 1f805ee95b..b3e2f55742 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -174,7 +174,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; - realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? + // Timeout to consider commands old + double cmd_timeout_; + // Are we holding position? + realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d2d0dc9dbd..3bd4873a31 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -139,9 +139,6 @@ class Trajectory return trajectory_msg_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ad40fc2da8..5e39bffc73 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -186,7 +186,7 @@ controller_interface::return_type JointTrajectoryController::update( state_current_.time_from_start.set__sec(0); read_state_from_hardware(state_current_); - // currently carrying out a trajectory. + // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; @@ -211,12 +211,32 @@ controller_interface::return_type JointTrajectoryController::update( if (valid_point) { + const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + // this is the time instance + // - started with the first segment: when the first point will be reached (in the future) + // - later: when the point of the current segment was reached + const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start; + // time_difference is + // - negative until first point is reached + // - counting from zero to time_from_start of next point + double time_difference = time.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - double time_difference = 0.0; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + // have we reached the end, are not holding position, and is a timeout configured? + // Check independently of other tolerances + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && + time_difference > cmd_timeout_) + { + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); + } + // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) { @@ -243,12 +263,6 @@ controller_interface::return_type JointTrajectoryController::update( if (default_tolerances_.goal_time_tolerance != 0.0) { - // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = time.seconds() - traj_end.seconds(); - if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; @@ -894,6 +908,26 @@ 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; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 5a1d985a7a..ce17ba8bf9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -83,6 +83,14 @@ joint_trajectory_controller: default_value: true, description: "If false, the last velocity point has to be zero or the goal will be rejected", } + cmd_timeout: { + type: double, + default_value: 0.0, # seconds + description: "Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + If zero, timeout is deactivated", + } gains: __map_joints: p: { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e02215e842..bb387dbc4d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -565,6 +565,147 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) executor.cancel(); } +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + double cmd_timeout = 3.0; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief check if no timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, no_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + // value of velocities is different from above due to spline interpolation + EXPECT_GT(state_msg->reference.velocities[0], 0.0); + EXPECT_GT(state_msg->reference.velocities[1], 0.0); + EXPECT_GT(state_msg->reference.velocities[2], 0.0); + + executor.cancel(); +} + +/** + * @brief check if timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double cmd_timeout = 0.1; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); + subscribeToState(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // update until end of trajectory -> no timeout should have occurred + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should have the trajectory with three points + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + + // update until timeout should have happened + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // after timeout, set_hold_position adds a new trajectory + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should be not more than one point now (from hold position) + EXPECT_FALSE(traj_controller_->has_nontrivial_traj()); + // should hold last position with zero velocity + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are normalized if configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f25f2a03f8..cea61c92bf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -137,6 +137,13 @@ class TestableJointTrajectoryController return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; } + bool has_nontrivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + } + + double get_cmd_timeout() { return cmd_timeout_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From e763ef0dd58fd2bea36ef66692bcb2b76b7740ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 16 Sep 2023 21:02:59 +0200 Subject: [PATCH 356/524] [Docs] Improve interface description of JTC (#770) --- joint_trajectory_controller/doc/userdoc.rst | 27 ++++++++++----------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index b9d8e274cf..deedc4a7b6 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -14,16 +14,24 @@ Waypoints consist of positions, and optionally velocities and accelerations. *Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ -Hardware interface type [#f1]_ +Hardware interface types ------------------------------- -Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time: +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations: + +* ``position`` +* ``position``, ``velocity`` +* ``position``, ``velocity``, ``acceleration`` +* ``velocity`` +* ``effort`` + +This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time: * For command interfaces ``position``, the desired positions are simply forwarded to the joints, * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. -* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`). +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`). -This leads to the the following allowed combinations of command and state interfaces: +This leads to the following allowed combinations of command and state interfaces: * With command interface ``position``, there are no restrictions for state interfaces. * With command interface ``velocity``: @@ -32,12 +40,9 @@ This leads to the the following allowed combinations of command and state interf * no restrictions otherwise. * With command interface ``effort``, state interfaces must include ``position, velocity``. -* With command interface ``acceleration``, there are no restrictions for state interfaces. Example controller configurations can be found :ref:`below `. -Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). - Other features -------------- @@ -118,14 +123,8 @@ States ,,,,,,,,,,,,,,,,,, The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. - -Legal combinations of state interfaces are: -* ``position`` -* ``position`` and ``velocity`` -* ``position``, ``velocity`` and ``acceleration`` -* ``effort`` +Legal combinations of state interfaces are given in section `Hardware Interface Types`_. Commands ,,,,,,,,, From fc5454a5c2508cccd2858364465f792f98a2c4b9 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sun, 17 Sep 2023 15:23:39 +0100 Subject: [PATCH 357/524] Bump actions/upload-artifact from 3.1.2 to 3.1.3 (#774) --- .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 cc5dd32596..f3a096f40d 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -42,7 +42,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 5a01d23533..167386ceff 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -44,7 +44,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 84d988c78646a9cf34a6f06532504416f29a3873 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 17 Sep 2023 20:50:55 +0200 Subject: [PATCH 358/524] [Doc] Add specific documentation on the available fw cmd controllers (#765) --- effort_controllers/doc/userdoc.rst | 40 ++++++++++++++++++++-- forward_command_controller/doc/userdoc.rst | 12 ++++++- position_controllers/doc/userdoc.rst | 40 ++++++++++++++++++++-- velocity_controllers/doc/userdoc.rst | 40 ++++++++++++++++++++-- 4 files changed, 122 insertions(+), 10 deletions(-) diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 74caf63391..62e98a75f9 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -7,7 +7,41 @@ effort_controllers This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. -Hardware interface type ------------------------ +The package contains the following controllers: -These controllers work with joints using the "effort" command interface. +effort_controllers/JointGroupEffortController +------------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "effort" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,,,,,,,,,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' effort commands + + +Parameters +,,,,,,,,,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + effort_controller: + type: effort_controllers/JointGroupEffortController + + effort_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index a91479c9af..01aa2492a8 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -16,8 +16,18 @@ Hardware interface type This controller can be used for every type of command interface. + +ROS 2 interface of the controller +--------------------------------- + +Topics +^^^^^^^ + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Target joint commands + Parameters ------------- +^^^^^^^^^^^^^^ This controller uses the `generate_parameter_library `_ to handle its parameters. diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index f321e8a698..89766df9d4 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -7,7 +7,41 @@ position_controllers This is a collection of controllers that work using the "position" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the position on a certain joint to achieve a set velocity. -Hardware interface type ------------------------ +The package contains the following controllers: -These controllers work with joints using the "position" command interface. +position_controllers/JointGroupPositionController +------------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "position" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,,,,,,,,,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' position commands + + +Parameters +,,,,,,,,,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + position_controller: + type: position_controllers/JointGroupPositionController + + position_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index fa7e36062e..469b975a97 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -7,7 +7,41 @@ velocity_controllers This is a collection of controllers that work using the "velocity" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the velocity on a certain joint to achieve a set position. -Hardware interface type ------------------------ +The package contains the following controllers: -These controllers work with joints using the "velocity" command interface. +velocity_controllers/JointGroupVelocityController +------------------------------------------------- + +This is specialization of the :ref:`forward_command_controller ` that works using the "velocity" joint interface. + + +ROS 2 interface of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Topics +,,,,,,,,,,,,,,,,,, + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Joints' velocity commands + + +Parameters +,,,,,,,,,,,,,,,,,, +This controller overrides the interface parameter from :ref:`forward_command_controller `, and the +``joints`` parameter is the only one that is required. + +An example parameter file is given here + +.. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + velocity_controller: + ros__parameters: + joints: + - slider_to_cart From 43a9a0bba4160ad87226806c1d9d0be5cbcd90ad Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 20 Sep 2023 12:40:36 +0100 Subject: [PATCH 359/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 9 +++++++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 5 +++++ 20 files changed, 74 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 723ca88779..3a5780a430 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 593fa56562..64969ca7a0 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- * Update docs for diff drive controller (`#751 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index be6ac52313..195b484703 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 52c8572739..79c903b365 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.15.0 (2023-09-11) ------------------- * Update docs for diff drive controller (`#751 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4b93f1f69a..57635be0aa 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 296cad54bf..bd0168c08d 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.15.0 (2023-09-11) ------------------- * [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b3d33964b6..afe614402f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 82cf6cf45b..fa20eb1305 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- * Add test for effort gripper controller (`#769 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 915544197c..4fde46e25e 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.15.0 (2023-09-11) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 57bc1b7f45..2d25987021 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.15.0 (2023-09-11) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8b1a1c6a54..ad98fac23d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Docs] Improve interface description of JTC (`#770 `_) +* [JTC] Add time-out for trajectory interfaces (`#609 `_) +* [JTC] Rename parameter: normalize_error to angle_wraparound (`#772 `_) +* [JTC] Fix hold position mode with goal_time>0 (`#758 `_) +* [JTC] Add note on goal_time=0 in docs (`#773 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- * [JTC] Make most parameters read-only (`#771 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5da7a35e1a..c4697db65a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 6e63a22917..4c5850cff1 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.15.0 (2023-09-11) ------------------- * add a broadcaster for range sensor (`#725 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6684adfabc..d6c3429105 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- * add a broadcaster for range sensor (`#725 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 3caadb3880..78d6a6af53 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 180dd35033..ef5e35e33e 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.15.0 (2023-09-11) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 4c4dae365d..8eb6b6eb0e 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 625c0f4a06..f1a1dfe854 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index afe6360322..06bdd6e477 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.15.0 (2023-09-11) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 2551b76bd8..07df2c825e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- From 79c6d1cb37be84deeb9103448b5cf93b9c3b8327 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 20 Sep 2023 12:40:51 +0100 Subject: [PATCH 360/524] 3.16.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- 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 3a5780a430..d576811bdb 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 40a3545b99..f866089f1c 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.15.0 + 3.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 64969ca7a0..e17d56865d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9f2de80dad..43a7ab0aad 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.15.0 + 3.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 195b484703..7c652e7fa7 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index ddc110f81a..8707502915 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.15.0 + 3.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 79c903b365..d670ccc047 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index cc849d12ef..611dfbe15a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.15.0 + 3.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 57635be0aa..efc583475d 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) * Contributors: Christoph Fröhlich diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d0aca9701a..9e3cc8bc32 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index bd0168c08d..70fe56b159 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4cbad1c014..12ec3d1945 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index afe614402f..ca209a52f5 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.16.0 (2023-09-20) +------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index af90230c7e..c8d0b99a59 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fa20eb1305..bc66422a37 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d3c8d9bf9f..dcd778363e 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.15.0 + 3.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4fde46e25e..477134c496 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 31684ec958..6173569630 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 2d25987021..35298737f0 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 248dbcb099..05f761169a 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.15.0 + 3.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ad98fac23d..a322fef5ad 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.16.0 (2023-09-20) +------------------- * [Docs] Improve interface description of JTC (`#770 `_) * [JTC] Add time-out for trajectory interfaces (`#609 `_) * [JTC] Rename parameter: normalize_error to angle_wraparound (`#772 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 521aa0ea9d..d8f2e8e997 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.15.0 + 3.16.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 c4697db65a..06e2f3e8db 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) * Contributors: Christoph Fröhlich diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c947f00bdb..712d03068a 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 4c5850cff1..9d5242684a 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 33ed7d5e4d..70d9c1fa2f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index d6c3429105..d21abd09ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ea2456486b..35cbdbf9bd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.15.0 + 3.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 78d6a6af53..525d0a0221 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 845e65d233..398a279ae7 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.15.0 + 3.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e4d9a72f89..9b4e3a8b6b 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.15.0", + version="3.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ef5e35e33e..ecf0568408 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 9987061405..96b34a5609 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.15.0 + 3.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 0dea799d80..cf09679e00 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.15.0", + version="3.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8eb6b6eb0e..a7546cb59d 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 48c137460b..c7c654846a 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.15.0 + 3.16.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f1a1dfe854..91175391f3 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index a53be880f2..ec4d5e064d 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.15.0 + 3.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 06bdd6e477..dd50c98185 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.16.0 (2023-09-20) +------------------- 3.15.0 (2023-09-11) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f320c08c9f..64ebddd5e2 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.15.0 + 3.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 07df2c825e..ce8bcfefa3 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.16.0 (2023-09-20) +------------------- * [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) * Contributors: Christoph Fröhlich diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d0b6cf0ed1..e47e170226 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 3571c551bc28c10638386b1592aef3d662a34e03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Sep 2023 14:26:56 +0200 Subject: [PATCH 361/524] Update ci-coverage-build.yml (#781) --- .github/workflows/ci-coverage-build.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index f3a096f40d..d2dba1bce8 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -3,6 +3,9 @@ on: workflow_dispatch: branches: - master + push: + branches: + - master pull_request: branches: - master From f7c13ad25938246609b724104054754ec7fad18d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Sep 2023 14:38:46 +0200 Subject: [PATCH 362/524] [JTC] Add tests for acceleration command interface (#752) --- .../test/test_trajectory_controller.cpp | 113 +++++++++++++----- 1 file changed, 86 insertions(+), 27 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index bb387dbc4d..07d60ce077 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -57,8 +58,6 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } - void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) @@ -911,7 +910,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { - // don't check against a value, because spline intepolation might overshoot depending on + // don't check against a value, because spline interpolation might overshoot depending on // interface combinations EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); @@ -929,50 +928,82 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor); + std::vector points_positions = {1.0, 2.0, 3.0}; + std::vector jumble_map = {1, 2, 0}; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ - joint_names_[1], joint_names_[2], joint_names_[0]}; + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]], joint_names_[jumble_map[2]]}; + traj_msg.joint_names = jumbled_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); traj_msg.points[0].positions.resize(3); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 3.0; - traj_msg.points[0].positions[2] = 1.0; - - if (traj_controller_->has_velocity_command_interface()) + traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); + traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); + traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); + traj_msg.points[0].velocities.resize(3); + for (size_t i = 0; i < 3; i++) { - traj_msg.points[0].velocities.resize(3); - traj_msg.points[0].velocities[0] = -0.1; - traj_msg.points[0].velocities[1] = -0.1; - traj_msg.points[0].velocities[2] = -0.1; + // factor 2 comes from the linear interpolation in the spline trajectory for constant + // acceleration + traj_msg.points[0].velocities[i] = + 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; } + trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update for 0.25 seconds + // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, + // otherwise acceleration is zero from the spline trajectory) // TODO(destogl): Make this time a bit shorter to increase stability on the CI? // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25)); + updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { + // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling + // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } - // TODO(anyone): add here checks for acceleration commands + + if (traj_controller_->has_acceleration_command_interface()) + { + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } } /** @@ -1003,9 +1034,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); traj_msg.points[0].velocities[0] = - copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); traj_msg.points[0].velocities[1] = - copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); trajectory_publisher_->publish(traj_msg); } @@ -1027,22 +1058,50 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the velocity // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } + if (traj_controller_->has_acceleration_command_interface()) + { + // estimate the sign of the acceleration + // joint rotates forward + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + } + EXPECT_NEAR(0.0, joint_acc_[2], threshold) + << "Joint 3 acc should be 0.0 since it's not in the goal"; + } + if (traj_controller_->has_effort_command_interface()) { // estimate the sign of the effort // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); EXPECT_NEAR(0.0, joint_eff_[2], threshold) << "Joint 3 effort should be 0.0 since it's not in the goal"; } - // TODO(anyone): add here checks for acceleration commands executor.cancel(); } @@ -1284,7 +1343,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = - copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + std::copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal From ca38c06f712663449ddf1372234d74a983b7d5f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 29 Sep 2023 20:07:47 +0200 Subject: [PATCH 363/524] Improve docs (#785) --- ackermann_steering_controller/doc/userdoc.rst | 2 ++ bicycle_steering_controller/doc/userdoc.rst | 2 ++ .../src/steering_controllers_library.yaml | 12 ++++++------ tricycle_steering_controller/doc/userdoc.rst | 2 ++ 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst index c4ae35e5b0..daf4561861 100644 --- a/ackermann_steering_controller/doc/userdoc.rst +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -19,4 +19,6 @@ This controller uses the `generate_parameter_library `, this controller adds the following parameters: + .. generate_parameter_library_details:: ../src/ackermann_steering_controller.yaml diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst index a720ff887d..6fccaf7b73 100644 --- a/bicycle_steering_controller/doc/userdoc.rst +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -20,4 +20,6 @@ This controller uses the `generate_parameter_library `, this controller adds the following parameters: + .. generate_parameter_library_details:: ../src/bicycle_steering_controller.yaml diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 86acb01dae..8acdfb1448 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -59,7 +59,7 @@ steering_controllers_library: open_loop: { type: bool, default_value: false, - description: "bool parameter decides if open oop or not (feedback).", + description: "Choose if open-loop or not (feedback) is used for odometry calculation.", read_only: false, } @@ -87,7 +87,7 @@ steering_controllers_library: enable_odom_tf: { type: bool, default_value: true, - description: "Publishing to tf is enabled or disabled ?.", + description: "Publishing to tf is enabled or disabled?", read_only: false, } @@ -108,15 +108,15 @@ steering_controllers_library: position_feedback: { type: bool, default_value: false, - description: "Choice of feedback type, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if - position_feedback is true then HW_IF_POSITION is taken as interface type", + description: "Choice of feedback type, if position_feedback is false then ``HW_IF_VELOCITY`` is taken as interface type, if + position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } use_stamped_vel: { type: bool, default_value: false, - description: "Choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if - use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if + use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", read_only: false, } diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst index d76f489745..618d7f04a7 100644 --- a/tricycle_steering_controller/doc/userdoc.rst +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -19,4 +19,6 @@ This controller uses the `generate_parameter_library `, this controller adds the following parameters: + .. generate_parameter_library_details:: ../src/tricycle_steering_controller.yaml From 26a130b8fef94854d94b866b3e5d272e6bf29262 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 3 Oct 2023 07:49:22 +0100 Subject: [PATCH 364/524] Bump actions/setup-python from 4.7.0 to 4.7.1 (#788) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index c868650adb..5d801016f9 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.0 + - uses: actions/setup-python@v4.7.1 with: python-version: '3.10' - name: Install system hooks From f62fc3ac5f480b5a74765e642cd33d17600f7f5a Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Fri, 6 Oct 2023 04:04:59 -0400 Subject: [PATCH 365/524] writing_new_controller docs fix (#790) * fixing structure * format * typo fix --- doc/writing_new_controller.rst | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0475f8f4fd..501c231def 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -34,10 +34,12 @@ The following is a step-by-step guide to create source files, basic tests, and c 3. Define a unique namespace for your controller. This is usually a package name written in ``snake_case``. 4. Define the class of the controller, extending ``ControllerInterface``, e.g., + .. code:: c++ - class ControllerName : public controller_interface::ControllerInterface - 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. + class ControllerName : public controller_interface::ControllerInterface + + 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. @@ -48,9 +50,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Include the header file of your controller and add a namespace definition to simplify further development. 2. (optional) Implement a constructor if needed. There, you could initialize member variables. - This could also be done in the ``init`` method. + This could also be done in the ``on_init`` method. - 3. Implement the ``init`` method. The first line usually calls the parent ``init`` method. + 3. Implement the ``on_init`` method. The first line usually calls the parent ``on_init`` method. Here is the best place to initialize the variables, reserve memory, and most importantly, declare node parameters used by the controller. If everything works fine return ``controller_interface::return_type::OK`` or ``controller_interface::return_type::ERROR`` otherwise. 4. Write the ``on_configure`` method. Parameters are usually read here, and everything is prepared so that the controller can be started. @@ -105,6 +107,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Add ament dependencies needed by the library. You should add at least those listed under 1. 5. Export for pluginlib description file using the following command: + .. code:: cmake pluginlib_export_plugin_description_file(controller_interface .xml) 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 366/524] 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 367/524] 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 368/524] [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 369/524] [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 370/524] 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 371/524] 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 372/524] 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 373/524] 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 374/524] 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 375/524] [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 376/524] 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 377/524] 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 378/524] [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 379/524] [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 380/524] [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 381/524] [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 382/524] [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 383/524] [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 384/524] [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 From 0a9f4b0c2b10c926d6c283520d763eefed0f06b5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 16 Nov 2023 23:17:20 +0000 Subject: [PATCH 385/524] Bump ros-tooling/action-ros-ci from 0.3.4 to 0.3.5 (#833) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 46922ccaac..76b2c22e53 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 723654b33e..1aaf30c639 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6a932a2dcd..72aa9af66c 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index a444a7f645..6b1af2b86c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From 25f2a14757e5624eabfd5804fb64b42100e72b52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 15:31:39 +0100 Subject: [PATCH 386/524] Resort overview page (#846) --- README.md | 2 +- doc/controllers_index.rst | 55 ++++++++++++---------- forward_command_controller/doc/userdoc.rst | 2 +- 3 files changed, 32 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index d41ec09b6c..4183b38fdc 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) -Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Navigation2. +Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. ## Build status diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index de33c3284c..9fceb90fd3 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -6,24 +6,9 @@ ros2_controllers ################# -`GitHub Repository `_ - - -Nomenclature -************ - -The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using. -The controllers are using `common hardware interface definitions`_. -The controllers' namespaces are commanding the following command interface types: - - - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` - - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` - - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` - - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - - ... - -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. +`Link to GitHub Repository `_ Guidelines and Best Practices @@ -36,34 +21,54 @@ Guidelines and Best Practices * -Available Controllers -********************* +Controllers for Mobile Robots +***************************** .. toctree:: :titlesonly: Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Admittance Controller <../admittance_controller/doc/userdoc.rst> Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> + +Controllers for Manipulators and Other Robots +********************************************* + +The controllers are using `common hardware interface definitions`_, and may use namespaces depending on the following command interface types: + + - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` + - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` + - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` + - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` + +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp + + +.. toctree:: + :titlesonly: + + Admittance Controller <../admittance_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> - Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> - Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> -Available Broadcasters +Broadcasters ********************** +Broadcasters are used to publish sensor data from hardware components to ROS topics. +In the sense of ros2_control, broadcasters are still controllers using the same controller interface as the other controllers above. + .. toctree:: :titlesonly: Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> - Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 01aa2492a8..cd623a5acb 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ forward_command_controller ========================== -This is a base class implementing a feedforward controller. Specific implementations can be found in: +This is a base class implementing a feedforward controller. Specific implementations of this base class can be found in: * :ref:`position_controllers_userdoc` * :ref:`velocity_controllers_userdoc` From 41610fdf43b9875f079aa5933b3f95e9687e524d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:17:24 +0100 Subject: [PATCH 387/524] [JTC] Remove unused home pose (#845) --- .../joint_trajectory_controller.hpp | 2 -- .../src/joint_trajectory_controller.cpp | 28 ------------------- 2 files changed, 30 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 7542aa6951..397ccf347c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -184,8 +184,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3936da155f..18f61f0e49 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -921,22 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); @@ -1028,8 +1013,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return CallbackReturn::SUCCESS; } @@ -1037,13 +1020,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { - // go home - if (traj_home_point_ptr_ != nullptr) - { - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_external_point_ptr_ = traj_home_point_ptr_; - } - return CallbackReturn::SUCCESS; } @@ -1070,11 +1046,7 @@ bool JointTrajectoryController::reset() } } - // iterator has no default value - // prev_traj_point_ptr_; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return true; } From d9a6bb43e312d8d74e52eddfb700e5249c18f4f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:18:46 +0100 Subject: [PATCH 388/524] [JTC] Fix dynamic reconfigure of tolerances (#849) * Fix dynamic reconfigure of tolerances * Fix static cast syntax --- .../src/joint_trajectory_controller.cpp | 5 +- .../test/test_trajectory_controller.cpp | 53 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 5 ++ 3 files changed, 61 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 18f61f0e49..dae7f13148 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -119,11 +119,12 @@ controller_interface::return_type JointTrajectoryController::update( if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - // use_closed_loop_pid_adapter_ is updated in on_configure only + default_tolerances_ = get_segment_tolerances(params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) { update_pids(); - default_tolerances_ = get_segment_tolerances(params_); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7842d50434..8b29c2cb16 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -449,6 +449,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) executor.cancel(); } +/** + * @brief check if dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.joint1.trajectory", 1.0), + rclcpp::Parameter("constraints.joint2.trajectory", 2.0), + rclcpp::Parameter("constraints.joint3.trajectory", 3.0), + rclcpp::Parameter("constraints.joint1.goal", 10.0), + rclcpp::Parameter("constraints.joint2.goal", 20.0), + rclcpp::Parameter("constraints.joint3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateController(); + + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 224265ad83..fdf4305cec 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -132,6 +132,11 @@ class TestableJointTrajectoryController std::vector get_pids() const { return pids_; } + joint_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const From 6d4fb2d9c5dc44751a19b277054ffb570406eebc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 22:33:01 +0100 Subject: [PATCH 389/524] fix tests for API break of passing controller manager update rate in init method (#854) --- .../test_ackermann_steering_controller.hpp | 2 +- .../test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_diff_drive_controller.cpp | 34 +++++++++---------- .../test_joint_group_effort_controller.cpp | 2 +- .../test_force_torque_sensor_broadcaster.cpp | 2 +- .../test/test_forward_command_controller.cpp | 2 +- ...i_interface_forward_command_controller.cpp | 2 +- .../test/test_gripper_controllers.cpp | 2 +- .../test/test_imu_sensor_broadcaster.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 2 +- .../test/test_trajectory_controller_utils.hpp | 2 +- .../test_joint_group_position_controller.cpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_controller.cpp | 14 ++++---- .../test_tricycle_steering_controller.hpp | 2 +- .../test_joint_group_velocity_controller.cpp | 2 +- 18 files changed, 40 insertions(+), 40 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 8af2f6bc94..a2849d5742 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index db708db6c5..19908d7f9f 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -185,7 +185,7 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", "", options); + auto result = controller_->init(controller_name, "", 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 34d6883a83..521506762b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 505aa44d2b..eb970d34a3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -195,7 +195,7 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -203,7 +203,7 @@ TEST_F(TestDiffDriveController, configure_fails_without_parameters) TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -223,7 +223,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -239,7 +239,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -259,7 +259,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -292,7 +292,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -327,7 +327,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -363,7 +363,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -398,7 +398,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -435,7 +435,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, test_namespace); + const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); std::string odom_id = "odom"; @@ -469,7 +469,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -483,7 +483,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -499,7 +499,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -516,7 +516,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -533,7 +533,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -550,7 +550,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -599,7 +599,7 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 23555c578f..f9d72ab202 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,7 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", ""); + const auto result = controller_->init("test_joint_group_effort_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 82f677410e..bfe7561642 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -53,7 +53,7 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", ""); + const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 2ab507ef29..62d5512b4e 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -60,7 +60,7 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", ""); + const auto result = controller_->init("forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 00ca4ae2d1..7826c85355 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -62,7 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", ""); + const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 572e755743..0fc10c0c73 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -60,7 +60,7 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", ""); + const auto result = controller_->init("gripper_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0b782efc5f..0fb987ce77 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -53,7 +53,7 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", ""); + const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index a6c0708fd9..3a74f599b4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,7 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init("joint_state_broadcaster", ""); + const auto result = state_broadcaster_->init("joint_state_broadcaster", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index fdf4305cec..3a7e7e5cf1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -196,7 +196,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", "", node_options); + auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); if (ret != controller_interface::return_type::OK) { FAIL(); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 07f42e4eba..3b4f00be12 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,7 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", ""); + const auto result = controller_->init("test_joint_group_position_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ab937146cb..a857141ea9 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -34,7 +34,7 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init(broadcaster_name, ""); + result = range_broadcaster_->init(broadcaster_name, "", 0); if (controller_interface::return_type::OK == result) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 226d4f4291..5caf347ac1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 054fff10ce..504ca381ce 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -173,14 +173,14 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, configure_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -200,7 +200,7 @@ TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -213,7 +213,7 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -227,7 +227,7 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); // We implicitly test that by default position feedback is required @@ -243,7 +243,7 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( @@ -292,7 +292,7 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_); + const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::OK); controller_->get_node()->set_parameter( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e9d1023d98..c5f6985d4e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 185c630bc9..4cbf1b7342 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,7 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", ""); + const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From ba7405443b1cdb78c0e0f3ec458738da37d690d0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:35 +0000 Subject: [PATCH 390/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 6 ++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 7 +++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 6 ++++++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 6 ++++++ 20 files changed, 119 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index d679a782d4..7b9a59976e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ba5a131094..1a359ce855 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 7b14b2d617..f18f0188a7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b3f4b127e3..6862ea574c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) +* [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota, Tony Baltovski + 3.17.0 (2023-10-31) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index baaada7c22..15c3edef1b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9b6dea8ef9..8bc4a07db2 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4bc9d41c74..77f249ccbd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Resort overview page (`#846 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b7c7242520..a59b2c24ce 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 8644b008a0..d1fdff1378 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index fd56b72c3b..456e81090b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9eece3af21..66d24ea513 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) +* [JTC] Remove unused home pose (`#845 `_) +* [JTC] Activate update of dynamic parameters (`#761 `_) +* [JTC] Fix tests when state offset is used (`#797 `_) +* [JTC] Remove deprecation warnings, set `allow_nonzero_velocity_at_trajectory_end` default false (`#834 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota, Dr Denis + 3.17.0 (2023-10-31) ------------------- * Cleanup comments and unnecessary checks (`#803 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5239c43407..b214480e86 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b1084b4a24..1f04cf47e7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 187db3d351..873dd5b55f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index acca28227f..0d1e8ad2ef 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- * [TestNodes] Optimize output about setup of JTC publisher (`#792 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index e723d39c6f..7263a948d5 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.17.0 (2023-10-31) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8082dc9070..5279d003cf 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Steering controllers library: fix open loop mode (`#793 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bda1c89d32..605a4f896d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a7e6854ea8..a41ee0623c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- * Improve docs (`#785 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 358ee9f499..9d4102e69a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix tests for API break of passing controller manager update rate in init method (`#854 `_) +* Adjust tests after passing URDF to controllers (`#817 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.17.0 (2023-10-31) ------------------- From 0d3fc520831d10d4c1d16784e7378c2a0bf7b10f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:52:59 +0000 Subject: [PATCH 391/524] 4.0.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7b9a59976e..7a94360e4b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 656c88feae..421f098f96 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.17.0 + 4.0.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1a359ce855..b94892ac83 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 62cf127d9f..a5f7e7e353 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.17.0 + 4.0.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index f18f0188a7..278921e18b 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 9ee4c53049..e3063672d6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.17.0 + 4.0.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6862ea574c..5f57b7e833 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) * [diff_drive_controller] Remove non-stamped Twist option (`#812 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 01dc69413a..56b9552f93 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.17.0 + 4.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 15c3edef1b..6c7d7d6fa3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d0627bea10..ef2610d34f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8bc4a07db2..17a6d4f280 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 81c865ddfb..4a1df9ff11 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 77f249ccbd..941b849edd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Resort overview page (`#846 `_) * Adjust tests after passing URDF to controllers (`#817 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 246bc6777d..0fe44110b1 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a59b2c24ce..f35dcd1a32 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 391bd1bb7d..08b0298e1f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.17.0 + 4.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d1fdff1378..4d2d2d01b3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 2dcacc1f0f..1819b974e2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 456e81090b..ce89b01912 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e4fecb14d7..1364b1a164 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.17.0 + 4.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 66d24ea513..a4486e4e98 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * [JTC] Fix dynamic reconfigure of tolerances (`#849 `_) * [JTC] Remove unused home pose (`#845 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f4efc2c060..fe10d9583d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.17.0 + 4.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b214480e86..26462d51a9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b3cd9b6cfb..2112f7d8c5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 1f04cf47e7..71ee9a7119 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index cf6545d65e..3ece4bda4b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.17.0 + 4.0.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 873dd5b55f..2c2e5ec358 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 49c87b152d..6a213308c5 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.17.0 + 4.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0d1e8ad2ef..e8eaf9c382 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index ef8c7b572d..5698930bff 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.17.0 + 4.0.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 102c39b607..a129b2d0a7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7263a948d5..0c08780904 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.17.0 (2023-10-31) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 08f885aef3..41cf5e85d7 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.17.0 + 4.0.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index c0f4a96c2a..5a5b979d41 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.17.0", + version="4.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 5279d003cf..1a76469d6c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 0194cfd041..932fda9a35 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.17.0 + 4.0.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 605a4f896d..6bc3d26f53 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9a6cf3ef2d..a663dcacde 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.17.0 + 4.0.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a41ee0623c..0ade9bca61 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index e39971982c..7f9d14c2fe 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.17.0 + 4.0.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 9d4102e69a..bb7e231222 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) * Adjust tests after passing URDF to controllers (`#817 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f336606b26..6ce13e6adf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.17.0 + 4.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2854a0bc5da016b6ddc55383e6f7bd7b8e1e57ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 26 Nov 2023 22:06:17 +0100 Subject: [PATCH 392/524] Use testing and remove wrong field of workflow_dispatch (#860) --- .github/workflows/humble-abi-compatibility.yml | 4 +--- .github/workflows/iron-abi-compatibility.yml | 4 +--- .github/workflows/rolling-abi-compatibility.yml | 4 +--- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 708ea5c1f4..5c288fabfb 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index 20d93f5af1..ab6642625f 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Iron - ABI Compatibility Check on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: iron - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 3911434a23..73055ef3e5 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,8 +1,6 @@ name: Rolling - ABI Compatibility Check on: workflow_dispatch: - branches: - - master pull_request: branches: - master @@ -15,6 +13,6 @@ jobs: - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: rolling - ROS_REPO: main + ROS_REPO: testing ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} NOT_TEST_BUILD: true From b8532284853c990a83741b85983f18ed38ef5f89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:31:10 +0100 Subject: [PATCH 393/524] [JTC] Improve update methods for tests (#858) --- .../test/test_trajectory_actions.cpp | 16 +- .../test/test_trajectory_controller.cpp | 967 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 143 ++- 3 files changed, 565 insertions(+), 561 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3b80abcb9b..ed13a24485 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -242,7 +242,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -298,7 +298,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -350,7 +350,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -409,7 +409,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -460,7 +460,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -509,7 +509,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -555,7 +555,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -603,7 +603,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position, // i.e., active but trivial trajectory (one point only) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8b29c2cb16..7c026d5e7a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" @@ -103,77 +101,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter different than joint_names_ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -316,6 +257,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; @@ -422,7 +368,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); auto pids = traj_controller_->get_pids(); if (traj_controller_->use_closed_loop_pid_adapter()) @@ -433,7 +379,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) double kp = 1.0; SetPidParameters(kp); - updateController(); + updateControllerAsync(); pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); @@ -458,7 +404,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -486,7 +432,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -513,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup without start_with_holding being set, we expect no active trajectory ASSERT_FALSE(traj_controller_->has_active_traj()); @@ -531,7 +477,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup with start_with_holding being set, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup @@ -552,7 +498,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -568,84 +513,206 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are angle_wraparound if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] angle_wraparound? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + if (traj_controller_->has_velocity_command_interface()) + { // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // no normalization of position error + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); @@ -685,13 +752,13 @@ TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) /** * @brief check if no timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, no_timeout) { rclcpp::executors::MultiThreadedExecutor executor; // zero is default value, just for demonstration rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -707,28 +774,27 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); // is the trajectory still active? EXPECT_TRUE(traj_controller_->has_active_traj()); // should still hold the points from above EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); - EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); // value of velocities is different from above due to spline interpolation - EXPECT_GT(state_msg->reference.velocities[0], 0.0); - EXPECT_GT(state_msg->reference.velocities[1], 0.0); - EXPECT_GT(state_msg->reference.velocities[2], 0.0); + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); executor.cancel(); } @@ -736,6 +802,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) /** * @brief check if timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, timeout) { rclcpp::executors::MultiThreadedExecutor executor; @@ -743,7 +810,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); double kp = 1.0; // activate feedback control for testing velocity/effort PID SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); - subscribeToState(); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -768,11 +834,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -792,130 +853,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -/** - * @brief check if position error of revolute joints are angle_wraparound if configured so - */ -TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) -{ - rclcpp::executors::MultiThreadedExecutor executor; - constexpr double k_p = 10.0; - std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); - subscribeToState(); - - size_t n_joints = joint_names_.size(); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - - const auto allowed_delta = 0.1; - - // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); - - // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); - - // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - - // is error.positions[2] angle_wraparound? - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], - state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - } - } - - if (traj_controller_->has_effort_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } - } - - executor.cancel(); -} - /** * @brief check if use_closed_loop_pid is active */ @@ -944,7 +881,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -960,35 +896,34 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); - EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); - EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); } if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); } // no change in state interface should happen if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -998,9 +933,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations - EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); - EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); - EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); } executor.cancel(); @@ -1016,6 +951,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ @@ -1025,29 +961,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(3); traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); - for (size_t i = 0; i < 3; i++) + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) { - // factor 2 comes from the linear interpolation in the spline trajectory for constant - // acceleration - traj_msg.points[0].velocities[i] = - 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, - // otherwise acceleration is zero from the spline trajectory) - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1068,19 +1002,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_acceleration_command_interface()) { - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); } if (traj_controller_->has_effort_command_interface()) @@ -1106,37 +1030,42 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(2); traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1148,7 +1077,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1156,24 +1085,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) - << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) - << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - } - EXPECT_NEAR(0.0, joint_acc_[2], threshold) + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1185,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1227,47 +1147,45 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe traj_controller_->wait_for_trajectory(executor); // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1408,8 +1326,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); - std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; @@ -1424,8 +1340,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1440,7 +1356,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities[1] = 0.0; expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1450,7 +1367,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1464,7 +1380,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0] trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time @@ -1473,14 +1390,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1493,19 +1410,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = - rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); // it should not have accepted the new goal but finish the old one expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1513,7 +1431,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1538,7 +1455,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); @@ -1551,28 +1469,21 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } -// TODO(destogl) this test fails with errors -// second publish() gives an error, because end time is before current time -// as well as -// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, -// which exceeds COMMON_THRESHOLD, where -// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, -// 2: joint_pos_[0] evaluates to 6.2700020099999998, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_jump_when_state_tracking_error_updated/0, where GetParam() = -// ({ "position" }, { "position" }) (3372 ms) - -#if 0 TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1585,95 +1496,95 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; - publish(time_from_start, points, - rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, - rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance + EXPECT_NEAR( + joint_state_pos_[0] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 0.1); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif - -// TODO(destogl) this test fails -// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, -// which exceeds COMMON_THRESHOLD, where -// 2: second_goal[0] evaluates to 6.5999999999999996, -// 2: joint_pos_[0] evaluates to 6.5670133649999993, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = -// ({ "position" }, { "position", "velocity" }) (3374 ms) -#if 0 + TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; + // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1684,77 +1595,79 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from - // command (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in the goal direction from - // command (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif // Testing that values are read from state interfaces when hardware is started for the first // time and hardware state has offset --> this is indicated by NaN values in command interfaces @@ -1902,7 +1815,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1934,7 +1847,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1942,7 +1855,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectHoldingPoint(hold_position); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3a7e7e5cf1..3c8e252067 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -29,7 +29,7 @@ namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -151,6 +151,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -233,21 +237,24 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor); - - // add this to simplify tests, can be overwritten by parameters - rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); - traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); - - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); - - // set optional parameters - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), + [](const rclcpp::Parameter & param) { + return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; + }) != parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -407,43 +414,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() < end_time) + while (clock.now() <= end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; } - void waitAndCompareState( + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -453,10 +502,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR( - expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getState() const @@ -573,6 +623,47 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_; From fe0c91d4f36939866276d63c1d8f552544f6818f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:32:27 +0100 Subject: [PATCH 394/524] joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (#859) --- .../test/test_joint_state_broadcaster.cpp | 21 +++++++++---------- .../test/test_joint_state_broadcaster.hpp | 3 +++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3a74f599b4..b44152a453 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -531,19 +531,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) state_broadcaster_->get_node()->set_parameter( {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - // configure ok - ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message("joint_states", joint_state_msg); const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); @@ -585,7 +578,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) controller_interface::return_type::OK); } -void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +void JointStateBroadcasterTest::activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) { auto node_state = state_broadcaster_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -616,9 +610,14 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st "controller/broadcaster update loop"; // take message from subscription - sensor_msgs::msg::JointState joint_state_msg; rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); +} + +void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +{ + sensor_msgs::msg::JointState joint_state_msg; + activate_and_get_joint_state_message(topic, joint_state_msg); const size_t NUM_JOINTS = joint_names_.size(); ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 63cc7a5483..fcaa40e8d5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -71,6 +71,9 @@ class JointStateBroadcasterTest : public ::testing::Test void test_published_dynamic_joint_state_message(const std::string & topic); + void activate_and_get_joint_state_message( + const std::string & topic, sensor_msgs::msg::JointState & msg); + protected: // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; From b6841ead459f6efb76153e8474a3f5443d48d2b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 27 Nov 2023 12:54:41 +0100 Subject: [PATCH 395/524] Increase test coverage of interface configuration getters (#856) --- .../test_force_torque_sensor_broadcaster.cpp | 25 +++++++- .../test_force_torque_sensor_broadcaster.hpp | 2 +- .../test/test_forward_command_controller.cpp | 42 +++++++++++++ ...i_interface_forward_command_controller.cpp | 23 +++++++ .../test/test_gripper_controllers.cpp | 12 ++++ .../test/test_imu_sensor_broadcaster.cpp | 23 +++++++ .../test/test_joint_state_broadcaster.cpp | 62 ++++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 4 +- .../test/test_range_sensor_broadcaster.cpp | 27 +++++++- .../test/test_tricycle_controller.cpp | 15 +++++ 10 files changed, 228 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index bfe7561642..ce6a04ec1f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -157,6 +159,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -175,7 +183,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { SetUpFTSBroadcaster(); @@ -186,6 +194,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + + // deactivate passed + ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = fts_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = fts_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index 5477b3fa6f..fe5b0ab3ba 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -36,7 +36,7 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty); - FRIEND_TEST(ForceTorqueSensorBroadcasterTest, ActivateSuccess); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); }; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 62d5512b4e..236cb14edd 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -34,6 +34,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -128,6 +130,13 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -173,9 +182,23 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -323,9 +346,22 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -344,6 +380,12 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7826c85355..2d3b61e211 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -36,6 +36,8 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -148,6 +150,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -282,6 +291,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes { SetUpController(true); + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // send command auto command_ptr = std::make_shared(); command_ptr->data = {10.0, 20.0, 30.0}; @@ -300,6 +316,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // check interface configuration + cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(3lu)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( controller_->rt_command_ptr_.readFromNonRT() && diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 0fc10c0c73..da9e15840e 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -31,6 +31,8 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; template void GripperControllerTest::SetUpTestCase() @@ -108,6 +110,16 @@ TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) ASSERT_EQ( this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT(cmd_if_conf.names, UnorderedElementsAre(std::string("joint_1/") + TypeParam::value)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0fb987ce77..83edc044d3 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -32,6 +32,8 @@ #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; +using testing::IsEmpty; +using testing::SizeIs; namespace { @@ -113,6 +115,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -126,6 +134,21 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + + // deactivate passed + ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = imu_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = imu_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index b44152a453..3e45740bbc 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -163,7 +163,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); } -TEST_F(JointStateBroadcasterTest, ActivateTest) +TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); @@ -177,6 +177,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTest) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -218,6 +224,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -259,6 +271,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) const size_t NUM_JOINTS = joint_names_.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -287,7 +305,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ElementsAreArray(interface_names_)); } -TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) +TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) { const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0]}; @@ -300,6 +318,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -329,6 +353,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface) dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); ASSERT_THAT( dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray(IF_NAMES)); + + // deactivate + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT( + state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -344,6 +378,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -412,6 +452,12 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); @@ -455,6 +501,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, SizeIs(0)); @@ -492,6 +544,12 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) const size_t NUM_JOINTS = JOINT_NAMES.size(); + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fcaa40e8d5..fa9d29c936 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -33,10 +33,10 @@ using hardware_interface::HW_IF_VELOCITY; class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTest); + FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); - FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface); + FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a857141ea9..7b8e6d0e02 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -22,6 +22,9 @@ #include "hardware_interface/loaned_state_interface.hpp" +using testing::IsEmpty; +using testing::SizeIs; + void RangeSensorBroadcasterTest::SetUp() { // initialize controller @@ -130,9 +133,15 @@ TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); } -TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) +TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) { init_broadcaster("test_range_sensor_broadcaster"); @@ -141,6 +150,22 @@ TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success) ASSERT_EQ( range_broadcaster_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + auto state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + + ASSERT_EQ( + range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + cmd_if_conf = range_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + state_if_conf = range_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 504ca381ce..d8beedae42 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -39,6 +39,7 @@ using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +using testing::UnorderedElementsAre; class TestableTricycleController : public tricycle_controller::TricycleController { @@ -209,6 +210,20 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT( + state_if_conf.names, + UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) From 35556854883b546143d7773c43053650a8b23840 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 30 Nov 2023 16:26:33 +0100 Subject: [PATCH 396/524] added documentation on common controller parameters (#855) --- doc/controllers_index.rst | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 9fceb90fd3..5f8780d961 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -72,3 +72,12 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + + +Common Controller Parameters +**************************** + +Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents `__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager. + +* ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager. +* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. From fcc0847958c17a942e2b233d3bea1ab2d2ac05dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 30 Nov 2023 17:36:36 +0100 Subject: [PATCH 397/524] [JTC] Activate checks for parameter validation (#857) --- .../test/test_trajectory_controller.cpp | 157 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 22 ++- 2 files changed, 91 insertions(+), 88 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7c026d5e7a..0b6651e79f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - traj_controller_->get_node()->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) ASSERT_EQ( state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); - ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ActivateTrajectoryController(); - - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -245,8 +242,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(false, deactivated_positions); - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(false, deactivated_positions); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // it should still be holding the position at time of deactivation @@ -1918,72 +1914,73 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); -// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` -// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -// { -// auto set_parameter_and_check_result = [&]() -// { -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// SetParameters(); // This call is replacing the way parameters are set via launch -// traj_controller_->get_node()->configure(); -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// }; -// -// SetUpTrajectoryController(false); -// -// // command interfaces: empty -// command_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // command interfaces: bad_name -// command_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // command interfaces: effort has to be only -// command_interface_types_ = {"effort", "position"}; -// set_parameter_and_check_result(); -// -// // command interfaces: velocity - position not present -// command_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // command interfaces: acceleration without position and velocity -// command_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: empty -// state_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // state interfaces: cannot not be effort -// state_interface_types_ = {"effort"}; -// set_parameter_and_check_result(); -// -// // state interfaces: bad name -// state_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // state interfaces: velocity - position not present -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: acceleration without position and velocity -// state_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // velocity-only command interface: position - velocity not present -// command_interface_types_ = {"velocity"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// -// // effort-only command interface: position - velocity not present -// command_interface_types_ = {"effort"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// } +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3c8e252067..b530517517 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}) + { + auto ret = SetUpTrajectoryControllerLocal(parameters); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + } + + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); @@ -200,12 +211,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); - if (ret != controller_interface::return_type::OK) - { - FAIL(); - } - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + return traj_controller_->init(controller_name_, "", 0, "", node_options); } void SetPidParameters( @@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test initial_eff_joints); } - void ActivateTrajectoryController( + rclcpp_lifecycle::State ActivateTrajectoryController( bool separate_cmd_and_state_values = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, @@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_node()->activate(); + return traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } From 62ce4879e45fdec2f462f14a695cf579afbfe866 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:39:06 +0100 Subject: [PATCH 398/524] [JTC] Remove start_with_holding option (#839) --- .../src/joint_trajectory_controller.cpp | 7 ++---- ...oint_trajectory_controller_parameters.yaml | 5 ---- .../test/test_trajectory_controller.cpp | 23 ++----------------- 3 files changed, 4 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dae7f13148..d24fd5a34a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -944,11 +944,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } - // Should the controller start by holding position at the beginning of active state? - if (params_.start_with_holding) - { - add_new_trajectory_msg(set_hold_position()); - } + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 87ca1daea5..4981489d36 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -73,11 +73,6 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } - start_with_holding: { - type: bool, - default_value: true, - description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", - } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b6651e79f..0b049000e3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -444,24 +444,6 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) executor.cancel(); } -/** - * @brief check if hold on startup is deactivated - */ -TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) -{ - rclcpp::executors::MultiThreadedExecutor executor; - - rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); - - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup without start_with_holding being set, we expect no active trajectory - ASSERT_FALSE(traj_controller_->has_active_traj()); - - executor.cancel(); -} - /** * @brief check if hold on startup */ @@ -469,12 +451,11 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) { rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup with start_with_holding being set, we expect an active trajectory: + // after startup, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; From e4f1700ef653268130e25e65e351dc4d5572b54d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:40:56 +0100 Subject: [PATCH 399/524] [JTC] Continue with last trajectory-point on success (#842) --- joint_trajectory_controller/doc/userdoc.rst | 4 +- .../joint_trajectory_controller.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 16 ++- .../test/test_trajectory_actions.cpp | 136 +++++++++++++----- .../test/test_trajectory_controller.cpp | 14 +- .../test/test_trajectory_controller_utils.hpp | 56 +++----- 6 files changed, 162 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 58ba734b45..6a34185437 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -150,12 +150,14 @@ Actions [#f1]_ /follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller - The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. + Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. + .. _Subscriber: Subscriber [#f1]_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 397ccf347c..d16e4f8267 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -174,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Timeout to consider commands old double cmd_timeout_; - // Are we holding position? + // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; @@ -244,9 +244,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d24fd5a34a..9306b07354 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -362,7 +362,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -1466,6 +1466,20 @@ JointTrajectoryController::set_hold_position() return hold_position_msg_ptr_; } +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + bool JointTrajectoryController::contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ed13a24485..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0) + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - point.positions = point_positions; points.push_back(point); @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(point_positions); - } - else + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions, point_velocities); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - point1.positions.resize(joint_names_.size()); - point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); - point2.positions.resize(joint_names_.size()); - point2.positions = points_positions.at(1); points.push_back(point2); @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(points_positions.at(1)); - } - else + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } /** @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(0)); + expectCommandPoint(points_positions.at(0)); } /** @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + expectCommandPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) // it should be holding the last position, // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(cancelled_position); + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b049000e3..05010c562c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // it should still be holding the position at time of deactivation // i.e., active but trivial trajectory (one point only) traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - expectHoldingPoint(deactivated_positions); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -459,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(initial_positions); executor.cancel(); } @@ -819,12 +819,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // should hold last position with zero velocity if (traj_controller_->has_position_command_interface()) { - expectHoldingPoint(points.at(2)); + expectCommandPoint(points.at(2)); } else { // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + expectCommandPoint(INITIAL_POS_JOINTS); } executor.cancel(); @@ -1795,7 +1795,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -1827,14 +1827,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point - expectHoldingPoint(hold_position); + expectCommandPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b530517517..4a65b4ad51 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -521,7 +521,8 @@ class TrajectoryControllerTest : public ::testing::Test return state_msg_; } - void expectHoldingPoint(std::vector point) + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -531,16 +532,16 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); } if (traj_controller_->has_acceleration_command_interface()) @@ -557,40 +558,29 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(0.0, joint_eff_[2]); } } - else + else // traj_controller_->use_closed_loop_pid_adapter() == true { // velocity or effort PID? - // velocity setpoint is always zero -> feedforward term does not have an effect // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", velocity command is " << joint_vel_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", velocity command is " << joint_vel_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", velocity command is " << joint_vel_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", effort command is " << joint_eff_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", effort command is " << joint_eff_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", effort command is " << joint_eff_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } } } } From 757310073cadb20e551c407354f5a40e09e39a2a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:13 +0000 Subject: [PATCH 400/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 20 files changed, 80 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7a94360e4b..ebd6d5ef43 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b94892ac83..19fbe5864d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 278921e18b..dbfe611d52 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5f57b7e833..e0166e9dd8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6c7d7d6fa3..963c8916db 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 17a6d4f280..005b4ba6ac 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 941b849edd..f65fefd016 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f35dcd1a32..3e06cc1830 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d2d2d01b3..b05b8ce192 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ce89b01912..9f80626bb2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a4486e4e98..1ad5b44a48 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Continue with last trajectory-point on success (`#842 `_) +* [JTC] Remove start_with_holding option (`#839 `_) +* [JTC] Activate checks for parameter validation (`#857 `_) +* [JTC] Improve update methods for tests (`#858 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 26462d51a9..caf8f3925c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 71ee9a7119..d15eb127e6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2c2e5ec358..9ae67679d7 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e8eaf9c382..c3f8302d06 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0c08780904..58694e910d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 1a76469d6c..b8f6854520 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 6bc3d26f53..9e67804591 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 0ade9bca61..bd2fc2d678 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bb7e231222..cb05fb45f1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) From 23f944f26e7e21764be3acdb6610cd59866c9bfc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:35 +0000 Subject: [PATCH 401/524] 4.1.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index ebd6d5ef43..a1cbe9596e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 421f098f96..476c4d3050 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.0.0 + 4.1.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 19fbe5864d..615a457992 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a5f7e7e353..da5b652bd0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.0.0 + 4.1.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index dbfe611d52..bee3606ec8 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index e3063672d6..c627577ee8 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.0.0 + 4.1.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e0166e9dd8..8ee42d701f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 56b9552f93..03dffe764d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.0.0 + 4.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 963c8916db..f4496d3927 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ef2610d34f..e55e095c57 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 005b4ba6ac..b87aaa2641 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4a1df9ff11..48db0e49b6 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index f65fefd016..b38f021587 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0fe44110b1..c500c1f714 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3e06cc1830..798a0f3e67 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 08b0298e1f..34c7ba663e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.0.0 + 4.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b05b8ce192..2c311626cf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 1819b974e2..3ab9c5fff6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9f80626bb2..f2295beca3 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1364b1a164..37ba7b4912 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.0.0 + 4.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1ad5b44a48..7e4c25a474 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) * [JTC] Remove start_with_holding option (`#839 `_) * [JTC] Activate checks for parameter validation (`#857 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fe10d9583d..b41a7cad0c 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.0.0 + 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index caf8f3925c..6f0870c9ae 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2112f7d8c5..1aec89e59d 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index d15eb127e6..be35427e46 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3ece4bda4b..222d87e3bb 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 9ae67679d7..46f994c792 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6a213308c5..d34a5b1375 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.0.0 + 4.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c3f8302d06..a7a2f7a1fd 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 5698930bff..16dd709c25 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.0.0 + 4.1.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a129b2d0a7..baa01b97c2 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 58694e910d..ee80d4b30d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 41cf5e85d7..24ed2c9d63 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.0.0 + 4.1.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 5a5b979d41..2d855d9255 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index b8f6854520..bae8e6e557 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 932fda9a35..5de0ddc872 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.0.0 + 4.1.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 9e67804591..0203298259 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index a663dcacde..5f382bf8d2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.0.0 + 4.1.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index bd2fc2d678..4abf0788fb 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7f9d14c2fe..f565ef0c02 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.0.0 + 4.1.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cb05fb45f1..91575e6075 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 6ce13e6adf..d7376e8e6b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 038a54794ef06147e7a4ab5298b8c9eabe65e287 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Dec 2023 20:01:15 +0100 Subject: [PATCH 402/524] =?UTF-8?q?=F0=9F=9A=80=20Add=20PID=20controller?= =?UTF-8?q?=20=F0=9F=8E=89=20(#434)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/controllers_index.rst | 1 + pid_controller/CMakeLists.txt | 108 ++++ pid_controller/README.md | 7 + pid_controller/doc/userdoc.rst | 86 +++ .../include/pid_controller/pid_controller.hpp | 141 +++++ .../pid_controller/visibility_control.h | 49 ++ pid_controller/package.xml | 35 ++ pid_controller/pid_controller.xml | 8 + pid_controller/src/pid_controller.cpp | 521 +++++++++++++++++ pid_controller/src/pid_controller.yaml | 87 +++ .../test/pid_controller_params.yaml | 11 + .../test/pid_controller_preceding_params.yaml | 11 + .../test/test_load_pid_controller.cpp | 43 ++ pid_controller/test/test_pid_controller.cpp | 531 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 285 ++++++++++ .../test/test_pid_controller_preceding.cpp | 103 ++++ ros2_controllers/package.xml | 1 + 17 files changed, 2028 insertions(+) create mode 100644 pid_controller/CMakeLists.txt create mode 100644 pid_controller/README.md create mode 100644 pid_controller/doc/userdoc.rst create mode 100644 pid_controller/include/pid_controller/pid_controller.hpp create mode 100644 pid_controller/include/pid_controller/visibility_control.h create mode 100644 pid_controller/package.xml create mode 100644 pid_controller/pid_controller.xml create mode 100644 pid_controller/src/pid_controller.cpp create mode 100644 pid_controller/src/pid_controller.yaml create mode 100644 pid_controller/test/pid_controller_params.yaml create mode 100644 pid_controller/test/pid_controller_preceding_params.yaml create mode 100644 pid_controller/test/test_load_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.hpp create mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5f8780d961..d042fe79dd 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..81cbe6f006 --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pid_controller_parameters + src/pid_controller.yaml +) + +add_library(pid_controller SHARED + src/pid_controller.cpp +) +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC + $ + $ +) +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters +) +ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_pid_controller + test/test_pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller PRIVATE include) + target_link_libraries(test_pid_controller pid_controller) + ament_target_dependencies( + test_pid_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_preceding + test/test_pid_controller_preceding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml + ) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) + ament_target_dependencies( + test_pid_controller_preceding + controller_interface + hardware_interface + ) + + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) + target_include_directories(test_load_pid_controller PRIVATE include) + ament_target_dependencies( + test_load_pid_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/pid_controller +) + +install(TARGETS + pid_controller + pid_controller_parameters + EXPORT export_pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..01e8fddac8 --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implementation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5570f523fe --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,86 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst + +.. _pid_controller_userdoc: + +PID Controller +-------------------------------- + +PID Controller implementation that uses PidROS implementation from `control_toolbox `_ package. +The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. + +Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example: + +- reference/state POSITION; command VELOCITY --> PI CONTROLLER +- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER + +- reference/state VELOCITY; command POSITION --> PD CONTROLLER +- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER + +- reference/state POSITION; command POSITION --> PID CONTROLLER +- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER +- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER +- reference/state EFFORT; command EFFORT --> PID CONTROLLER + +.. note:: + + Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)` for the same purpose by sending only one reference point into it. + Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. + +Execution logic of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics. +If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + +Using the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pluginlib-Library: pid_controller +Plugin name: pid_controller/PidController + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +If controller is not in chained mode (``in_chained_mode == false``): + +- /reference [control_msgs/msg/MultiDOFCommand] + +If controller parameter ``use_external_measured_states`` is true: + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +,,,,,,,,,,, + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +The PID controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/pid_controller.yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp new file mode 100644 index 0000000000..a34dc9f87f --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/multi_dof_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "pid_controller/visibility_control.h" +#include "pid_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace pid_controller +{ + +enum class feedforward_mode_type : std::uint8_t +{ + OFF = 0, + ON = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + PID_CONTROLLER__VISIBILITY_PUBLIC + PidController(); + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + +protected: + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector reference_and_state_dof_names_; + size_t dof_; + std::vector measured_state_values_; + + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector feedforward_gain_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> measured_state_; + + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // internal methods + void update_parameters(); + controller_interface::CallbackReturn configure_parameters(); + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h new file mode 100644 index 0000000000..1509364b5a --- /dev/null +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/package.xml b/pid_controller/package.xml new file mode 100644 index 0000000000..7d52121582 --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,35 @@ + + + + pid_controller + 0.0.0 + Controller based on PID implememenation from control_toolbox package. + Bence Magyar + Denis Štogl + Denis Štogl + Apache-2.0 + + ament_cmake + + generate_parameter_library + + angles + control_msgs + control_toolbox + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/pid_controller/pid_controller.xml b/pid_controller/pid_controller.xml new file mode 100644 index 0000000000..5b90741ae6 --- /dev/null +++ b/pid_controller/pid_controller.xml @@ -0,0 +1,8 @@ + + + + PidController ros2_control controller. + + + diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp new file mode 100644 index 0000000000..373e941d96 --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,521 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); + +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + msg->dof_names = dof_names; + msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); +} + +void reset_controller_measured_state_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + reset_controller_reference_msg(msg, dof_names); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(feedforward_mode_type::OFF); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::update_parameters() +{ + if (!param_listener_->is_old(params_)) + { + return; + } + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn PidController::configure_parameters() +{ + update_parameters(); + + if (!params_.reference_and_state_dof_names.empty()) + { + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; + } + else + { + reference_and_state_dof_names_ = params_.dof_names; + } + + if (params_.dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'dof_names' (%zu) and 'reference_and_state_dof_names' (%zu) parameters has to be " + "the same!", + params_.dof_names.size(), reference_and_state_dof_names_.size()); + return CallbackReturn::FAILURE; + } + + dof_ = params_.dof_names.size(); + + // TODO(destogl): is this even possible? Test it... + if (params_.gains.dof_names_map.size() != dof_) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", + params_.gains.dof_names_map.size(), dof_); + return CallbackReturn::FAILURE; + } + + pids_.resize(dof_); + + for (size_t i = 0; i < dof_; ++i) + { + // prefix should be interpreted as parameters prefix + pids_[i] = + std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_and_state_dof_names_.clear(); + pids_.clear(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&PidController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + input_ref_.writeFromNonRT(msg); + + // input state Subscriber and callback + if (params_.use_external_measured_states) + { + auto measured_state_callback = + [&](const std::shared_ptr msg) -> void + { + // TODO(destogl): Sort the input values based on joint and interface names + measured_state_.writeFromNonRT(msg); + }; + measured_state_subscriber_ = get_node()->create_subscription( + "~/measured_state", subscribers_qos, measured_state_callback); + } + std::shared_ptr measured_state_msg = + std::make_shared(); + reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); + measured_state_.writeFromNonRT(measured_state_msg); + + measured_state_values_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + auto set_feedforward_control_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + response->success = true; + }; + + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); + + try + { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Reserve memory in state publisher + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) + { + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::reference_callback(const std::shared_ptr msg) +{ + if (msg->dof_names.empty() && msg->values.size() == reference_and_state_dof_names_.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Reference massage does not have DoF names defined. " + "Assuming that value have order as defined state DoFs"); + auto ref_msg = msg; + ref_msg->dof_names = reference_and_state_dof_names_; + input_ref_.writeFromNonRT(ref_msg); + } + else if ( + msg->dof_names.size() == reference_and_state_dof_names_.size() && + msg->values.size() == reference_and_state_dof_names_.size()) + { + auto ref_msg = msg; // simple initialization + + // sort values in the ref_msg + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + + bool all_found = true; + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + auto found_it = + std::find(ref_msg->dof_names.begin(), ref_msg->dof_names.end(), msg->dof_names[i]); + if (found_it == msg->dof_names.end()) + { + all_found = false; + RCLCPP_WARN( + get_node()->get_logger(), "DoF name '%s' not found in the defined list of state DoFs.", + msg->dof_names[i].c_str()); + break; + } + + auto position = std::distance(ref_msg->dof_names.begin(), found_it); + ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; + } + + if (all_found) + { + input_ref_.writeFromNonRT(ref_msg); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) and/or values (%zu) is not matching the expected size (%zu).", + msg->dof_names.size(), msg->values.size(), reference_and_state_dof_names_.size()); + } +} + +controller_interface::InterfaceConfiguration PidController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.dof_names.size()); + for (const auto & dof_name : params_.dof_names) + { + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (params_.use_external_measured_states) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(dof_name + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + ++index; + } + } + + return reference_interfaces; +} + +bool PidController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn PidController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command (the same number as state interfaces) + reset_controller_reference_msg(*(input_ref_.readFromRT()), reference_and_state_dof_names_); + reset_controller_measured_state_msg( + *(measured_state_.readFromRT()), reference_and_state_dof_names_); + + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto current_ref = input_ref_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) + { + if (!std::isnan((*current_ref)->values[i])) + { + reference_interfaces_[i] = (*current_ref)->values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + { + reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + } + + (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type PidController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // check for any parameter updates + update_parameters(); + + if (params_.use_external_measured_states) + { + const auto measured_state = *(measured_state_.readFromRT()); + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = measured_state->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + } + } + } + else + { + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + measured_state_values_[i] = state_interfaces_[i].get_value(); + } + } + + for (size_t i = 0; i < dof_; ++i) + { + double tmp_command = std::numeric_limits::quiet_NaN(); + + // Using feedforward + if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) + { + // calculate feed-forward + if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + else + { + tmp_command = 0.0; + } + + double error = reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -picomputeCommand( + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + } + else + { + // use calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + } + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].error_dot = + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + // Command can store the old calculated values. This should be obvious because at least one + // another value is NaN. + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + } + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace pid_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pid_controller::PidController, controller_interface::ChainableControllerInterface) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml new file mode 100644 index 0000000000..f645738862 --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,87 @@ +pid_controller: + dof_names: { + type: string_array, + default_value: [], + description: "Specifies dof_names or axes used by the controller. If 'reference_and_state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + reference_and_state_dof_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interface: { + type: string, + default_value: "", + description: "Name of the interface used by the controller for writing commands to the hardware.", + read_only: true, + validation: { + not_empty<>: null, + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + } + } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead from state interfaces." + } + gains: + __map_dof_names: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + antiwindup: { + type: bool, + default_value: false, + description: "Antiwindup functionality." + } + i_clamp_max: { + type: double, + default_value: 0.0, + description: "Upper integral clamp. Only used if antiwindup is activated." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp. Only used if antiwindup is activated." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Gain for the feed-forward part." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (i.e., are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..7555cfc156 --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml new file mode 100644 index 0000000000..673abfe08e --- /dev/null +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: + - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp new file mode 100644 index 0000000000..3a75f6e170 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); + + rclcpp::shutdown(); +} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..11f703a1a4 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,531 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are not wrapped +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are wrapped +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp new file mode 100644 index 0000000000..ab32f5cb48 --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,285 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef TEST_PID_CONTROLLER_HPP_ +#define TEST_PID_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "pid_controller/pid_controller.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; +using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestablePidController : public pid_controller::PidController +{ + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(PidControllerTest, activate_success); + FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); + FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); + FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = pid_controller::PidController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return pid_controller::PidController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class PidControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + reference_and_state_dof_names_ = {"joint1state"}; + + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); + + for (size_t i = 0; i < dof_names_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + dof_names_[i], command_interface_, &dof_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); + size_t index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + state_ifs.emplace_back(state_itfs_.back()); + ++index; + } + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_pid_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands( + const std::vector & values = {0.45}, const std::vector & values_dot = {0.0}) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.dof_names = dof_names_; + msg.values = values; + msg.values_dot = values_dot; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool feedforward, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = feedforward; + + bool wait_for_service_ret = + feedforward_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Service is not available!"); + } + auto result = feedforward_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; + std::vector reference_and_state_dof_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr feedforward_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..e0d3051226 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d34a5b1375..3b77b7cc69 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -20,6 +20,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library From 71b06d01c1374914b93f202c80182210b385fb5f Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Mon, 4 Dec 2023 13:21:13 -0700 Subject: [PATCH 403/524] Fix floating point comparison in JTC (#879) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix floating point comparison in JTC * Fix format --------- Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9306b07354..59e2c408c5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1369,10 +1369,11 @@ bool JointTrajectoryController::validate_trajectory_msg( { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) { - if (trajectory.points.back().velocities.at(i) != 0.) + if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) { RCLCPP_ERROR( - get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + get_node()->get_logger(), + "Velocity of last trajectory point of joint %s is not zero: %.15f", trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); return false; } From 4b267648fa1ac10120cb597e7f6ee23593b6633d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Dec 2023 17:53:25 +0100 Subject: [PATCH 404/524] Bump ros-tooling/setup-ros from 0.7.0 to 0.7.1 (#881) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.7.0 to 0.7.1. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.7.0...0.7.1) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 76b2c22e53..f263cd2da3 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: humble steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 1aaf30c639..720f0416c9 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: iron steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 72aa9af66c..e55285b179 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 5789c2dee4..60cf153978 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 6b1af2b86c..f0ac7b447d 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v4 From 925a690fcb469cb705ec67185cd67c6f1481dd61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Dec 2023 22:52:01 +0100 Subject: [PATCH 405/524] Add pid_controller to CI jobs (#883) --- .github/workflows/ci-coverage-build-humble.yml | 1 + .github/workflows/ci-coverage-build-iron.yml | 1 + .github/workflows/ci-coverage-build.yml | 1 + .github/workflows/ci-ros-lint.yml | 2 ++ 4 files changed, 5 insertions(+) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index f263cd2da3..ece507aaa1 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 720f0416c9..8d77e65b6b 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index e55285b179..bf9ad847b4 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 60cf153978..f6b9c027c9 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -31,6 +31,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers @@ -69,6 +70,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers From e8a127f2b8bc6b5561f54d4afcb30a641f60ec3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Dec 2023 13:42:43 +0100 Subject: [PATCH 406/524] Fix rqt jtc bugs for continuous joints and other minor bugs (#890) --- .../joint_limits_urdf.py | 57 ++++++++++++------- .../joint_trajectory_controller.py | 10 +++- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7914a76b17..5655e12f7a 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -14,6 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +# Code inspired on the joint_state_publisher package by David Lu!!! +# https://github.com/ros/robot_model/blob/indigo-devel/ +# joint_state_publisher/joint_state_publisher/joint_state_publisher + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -33,21 +37,24 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): - global description - use_small = use_smallest_joint_limits - use_mimic = True - - # Code inspired on the joint_state_publisher package by David Lu!!! - # https://github.com/ros/robot_model/blob/indigo-devel/ - # joint_state_publisher/joint_state_publisher/joint_state_publisher - +def subscribe_to_robot_description(node, key="robot_description"): qos_profile = rclpy.qos.QoSProfile(depth=1) qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE - node.create_subscription(String, "/robot_description", callback, qos_profile) - rclpy.spin_once(node) + node.create_subscription(String, key, callback, qos_profile) + + +def get_joint_limits(node, use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + count = 0 + while description == "" and count < 10: + print("Waiting for the robot_description!") + count += 1 + rclpy.spin_once(node, timeout_sec=1.0) free_joints = {} dependent_joints = {} @@ -66,21 +73,27 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr name = child.getAttribute("name") try: limit = child.getElementsByTagName("limit")[0] - except IndexError: - continue - if jtype == "continuous": - minval = -pi - maxval = pi - else: try: minval = float(limit.getAttribute("lower")) maxval = float(limit.getAttribute("upper")) except ValueError: - continue - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - continue + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: + raise Exception( + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + except IndexError: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: tag = safety_tags[0] diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 99f43e125e..162977cdfe 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -29,7 +29,7 @@ from .utils import ControllerLister, ControllerManagerLister from .double_editor import DoubleEditor -from .joint_limits_urdf import get_joint_limits +from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description from .update_combo import update_combo # TODO: @@ -170,6 +170,9 @@ def __init__(self, context): self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() + # subscriptions + subscribe_to_robot_description(self._node) + # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) @@ -460,8 +463,9 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.claimed_interfaces: - name = interface.split("/")[0] - joint_names.append(name) + name = interface.split("/")[-2] + if name not in joint_names: + joint_names.append(name) return joint_names From 960e073185b15f330d5d9e1b7af2682cc7c2e8b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 8 Dec 2023 19:47:20 +0100 Subject: [PATCH 407/524] Use more mergify features (#888) --- .github/mergify.yml | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 3aaaab2001..692c346ab4 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,7 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion conditions: - base=master @@ -21,7 +20,29 @@ pull_request_rules: - name: Ask to resolve conflict conditions: - conflict - - author!=mergify + - author!=mergify[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: Ask to resolve conflict for backports + conditions: + - conflict + - author=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + + - name: development targets master branch + conditions: + - base!=master + - author!=bmagyar + - author!=dstogl + - author!=christophfroehlich + - author!=mergify[bot] + actions: + comment: + message: | + @{{author}}, all pull requests must be targeted towards the `master` development branch. + Once merged into `master`, it is possible to backport to @{{base}}, but it must be in `master` + to have these changes reflected into new distributions. From 999eae5491a79f9734f8d5e4e18ffea3e7a04e2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 9 Dec 2023 21:09:32 +0100 Subject: [PATCH 408/524] Update good-first-issue.md (#895) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 4a2664918a..4de9ad8d30 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -55,6 +55,6 @@ Don’t hesitate to ask questions or to get help if you feel like you are gettin Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) -* [ROS Answers](https://answers.ros.org/questions/) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) **Good luck with your first issue!** From cb56213c7836caa208c6049048088d33a72b6aff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 17:52:20 +0100 Subject: [PATCH 409/524] Add configs for humble and iron (#901) --- .github/dependabot.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc654..aafd67c236 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "iron" From 52d3d3f2f64c357361c8f99f018bbaa49584e27f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 18:05:29 +0100 Subject: [PATCH 410/524] Bump actions/setup-python from 4.7.1 to 5.0.0 (#906) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.7.1 to 5.0.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.7.1...v5.0.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 5d801016f9..9f090b48ca 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.1 + - uses: actions/setup-python@v5.0.0 with: python-version: '3.10' - name: Install system hooks From 0a96cbb513282880d2d64adfb8c3c8c0ffc84b5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 19:08:10 +0100 Subject: [PATCH 411/524] Update list of reviewers (#884) --- .github/reviewer-lottery.yml | 29 +++++++++++------------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 84b156f5a1..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,28 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 - christophfroehlich - DasRoteSkelett - - sgmurray - - harderthan - - jaron-l - - malapatiravi + - duringhof - erickisos - - sachinkum0009 - - qiayuanliao - - homalozoa - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas + - mcbed + - moriarty + - olivier-stasse - peterdavidfagan - - duringhof + - progtologist + - saikishor - VanshGehlot - - bijoua29 - - LukasMacha97 - - mcbed + - VX792 From e8e091da251778985ec1c36b24874f8ba5886d93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Dec 2023 08:12:43 +0100 Subject: [PATCH 412/524] mergify: Ignore dependabot[bot] (#916) --- .github/mergify.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/mergify.yml b/.github/mergify.yml index 692c346ab4..0a6e425a30 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -21,6 +21,7 @@ pull_request_rules: conditions: - conflict - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? @@ -40,6 +41,7 @@ pull_request_rules: - author!=dstogl - author!=christophfroehlich - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: | From 1d0d7531ef8d24ea52e761a872b8bf277652dd33 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 12 Dec 2023 09:51:22 +0100 Subject: [PATCH 413/524] Cleanup package.xml und clarify tests of JTC. (#889) * Cleanup package.xml und clarify tests of JTC. * Update joint_trajectory_controller/package.xml Co-authored-by: Bence Magyar --------- Co-authored-by: Bence Magyar --- joint_trajectory_controller/package.xml | 15 +++++---------- .../test/test_trajectory_controller_utils.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b41a7cad0c..70d9bd51d2 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -4,29 +4,24 @@ 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar - Jordan Palacios - Karsten Knese + Dr. Denis Štogl + Christoph Froehlich Apache License 2.0 ament_cmake - angles - pluginlib - realtime_tools - - angles - pluginlib - realtime_tools - + angles backward_ros controller_interface control_msgs control_toolbox generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle + realtime_tools rsl tl_expected trajectory_msgs diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4a65b4ad51..7b68d882ff 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -215,7 +215,7 @@ class TrajectoryControllerTest : public ::testing::Test } void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) + double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -223,13 +223,13 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", p_default); + const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_default); + prefix + ".angle_wraparound", angle_wraparound_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } From 364df81e9947671e6e51d45ce306b93afceebec8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:22 +0000 Subject: [PATCH 414/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 3 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 3 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 3 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 3 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 6 + pid_controller/CHANGELOG.rst | 164 ++++++++++++++++++ pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 5 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 5 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 22 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 pid_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a1cbe9596e..15e7408f00 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 615a457992..c1053639c0 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index bee3606ec8..89d3f6291d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8ee42d701f..646903a5ac 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index f4496d3927..7cf9e2f114 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b87aaa2641..027158d18c 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b38f021587..e23dc6db47 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 798a0f3e67..4c492457f2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2c311626cf..55c324f22c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f2295beca3..d46f807ab5 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 7e4c25a474..f2406abef0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup package.xml und clarify tests of JTC. (`#889 `_) +* Fix floating point comparison in JTC (`#879 `_) +* Contributors: Abishalini Sivaraman, Dr. Denis + 4.1.0 (2023-12-01) ------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst new file mode 100644 index 0000000000..af41de496e --- /dev/null +++ b/pid_controller/CHANGELOG.rst @@ -0,0 +1,164 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 7d52121582..c955c4747a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 0.0.0 + 4.1.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 6f0870c9ae..ef93a8b3d9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index be35427e46..b530b7c721 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 46f994c792..03064c6f95 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a7a2f7a1fd..b6dcdc8d3a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ee80d4b30d..5e0c8f82da 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) +* Contributors: Sai Kishor Kothakota + 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index bae8e6e557..44f3fa515f 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0203298259..98957eb42f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4abf0788fb..cb3a5af24d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 91575e6075..8b84ddf784 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ From 2bcff2f87505bbc32fdfaa8a2938cecddcadc3f8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:51 +0000 Subject: [PATCH 415/524] 4.2.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 15e7408f00..975ecbf5a6 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 476c4d3050..20bfca8003 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.1.0 + 4.2.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index c1053639c0..86ba49d92a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index da5b652bd0..478b10bb55 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.1.0 + 4.2.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 89d3f6291d..d7259fbc75 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c627577ee8..2573605890 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.1.0 + 4.2.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 646903a5ac..eb40ec1db1 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 03dffe764d..5cacf41dad 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.1.0 + 4.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 7cf9e2f114..6cd3b74a95 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e55e095c57..e42456c828 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 027158d18c..a808f82c70 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 48db0e49b6..154e597c5f 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e23dc6db47..45e5a8081c 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c500c1f714..324ec1c5be 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4c492457f2..ff2046a41b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 34c7ba663e..ce981141a5 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.1.0 + 4.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 55c324f22c..e8be87d1f3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 3ab9c5fff6..48a26ead3f 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index d46f807ab5..4e92131965 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 37ba7b4912..300f8b2238 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.1.0 + 4.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f2406abef0..1b3559af58 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * Cleanup package.xml und clarify tests of JTC. (`#889 `_) * Fix floating point comparison in JTC (`#879 `_) * Contributors: Abishalini Sivaraman, Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 70d9bd51d2..1d0929b8b5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.1.0 + 4.2.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index af41de496e..5924bc6d94 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/pid_controller/package.xml b/pid_controller/package.xml index c955c4747a..7caa3f7856 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.1.0 + 4.2.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ef93a8b3d9..2e1e38fa43 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 1aec89e59d..8ed752c536 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b530b7c721..c92b4867b5 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 222d87e3bb..08e8fd9506 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 03064c6f95..5839d43b68 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3b77b7cc69..793aea9140 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.1.0 + 4.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b6dcdc8d3a..d7c37b2fee 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 16dd709c25..84ebaea6fa 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.1.0 + 4.2.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index baa01b97c2..b9aa443081 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5e0c8f82da..67d9eb85df 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) * Contributors: Sai Kishor Kothakota diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 24ed2c9d63..4e7c622798 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.1.0 + 4.2.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 2d855d9255..1e49983693 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44f3fa515f..fa8cd4ee2f 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5de0ddc872..85398f1d90 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.1.0 + 4.2.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 98957eb42f..d7424ca31a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 5f382bf8d2..4f131da79c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.1.0 + 4.2.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index cb3a5af24d..3d04f2ba8d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f565ef0c02..f6eb3fff49 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.1.0 + 4.2.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8b84ddf784..98d6523639 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d7376e8e6b..3d901585db 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 13a43c32b5b4d0040c96d4e192db4f1ed6d4f046 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Wed, 13 Dec 2023 07:16:11 -0500 Subject: [PATCH 416/524] Changing default int values to double in steering controller's yaml file (#927) --- .../src/steering_controllers_library.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 8acdfb1448..a9f7fa75fb 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -1,7 +1,7 @@ steering_controllers_library: reference_timeout: { type: double, - default_value: 1, + default_value: 1.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } @@ -93,14 +93,14 @@ steering_controllers_library: twist_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of twist covariance matrix.", read_only: false, } pose_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of pose covariance matrix.", read_only: false, } From 691175034c8de94dc70318fa3edb9f3e889434d5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 21 Dec 2023 11:23:49 +0000 Subject: [PATCH 417/524] Bump actions/upload-artifact from 3.1.3 to 4.0.0 (#934) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index ece507aaa1..a1d159541d 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 8d77e65b6b..08e6eafd82 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index bf9ad847b4..f785652989 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f0ac7b447d..fcc1a297fd 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 4c6d7236245c527f275b1e03a7e5242b658dc782 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 21 Dec 2023 12:29:46 +0100 Subject: [PATCH 418/524] [JTC] Add console output for tolerance checks (#932) --- .../joint_trajectory_controller/tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b4cb029dd9..b5e660be54 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -150,7 +150,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 59e2c408c5..c6a5169855 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -247,20 +247,21 @@ controller_interface::return_type JointTrajectoryController::update( // Always check the state tolerance on the first sample in case the first sample // is the last point + // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.state_tolerance[index], + true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && + !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; @@ -269,6 +270,10 @@ controller_interface::return_type JointTrajectoryController::update( if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } } From 3c3f61437b89d53f01918d6e49f4428112609824 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Dec 2023 08:53:42 +0100 Subject: [PATCH 419/524] Do not run reviewer lottery for dependabot and mergify (#946) --- .github/workflows/reviewer_lottery.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 2edbc9b59e..ed28964e01 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -6,6 +6,7 @@ on: jobs: test: runs-on: ubuntu-latest + if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' steps: - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 From dcd15ee6bac003aecd668943f3d4c93c1e0ee171 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 29 Dec 2023 12:40:21 +0100 Subject: [PATCH 420/524] Add rqt_JTC to docs (#950) --- joint_trajectory_controller/doc/userdoc.rst | 1 + .../doc/rqt_joint_trajectory_controller.png | Bin 0 -> 29746 bytes rqt_joint_trajectory_controller/doc/userdoc.rst | 12 ++++++++++++ 3 files changed, 13 insertions(+) create mode 100644 rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png create mode 100644 rqt_joint_trajectory_controller/doc/userdoc.rst diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6a34185437..6fe2146802 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -193,6 +193,7 @@ Further information Trajectory Representation joint_trajectory_controller Parameters + rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst> .. rubric:: Footnote diff --git a/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png new file mode 100644 index 0000000000000000000000000000000000000000..cb86f4620bcc091f87db272c7868d16966b091b6 GIT binary patch literal 29746 zcmbTdbyU^s*Ds2JD5!{(AYl$F$c#zx0lPtV-O(86}>Mm-M- z$`cf!_itq#W7nr09A)KBf9-4yGx4Fnh<@-SiR_xUBp*cy>NPDn4yqzbwFgoP{PJ(! zE~fied*=M2O8!76Eb}H6`wa(HX6Vi9Ld$nnqOCnVbQvEq2$5?^e>~dbI{FknIb*ZQ zji|Fxd2{=z_XEEFi$BKS$~B&-UgQ0B!}2=Qeg6;0k0iTBf1-XuP=Wpv-{D$|Sb^-D zZ`IWnS3kSe6~^X$+q`k9(e5zk?xZ!9d_Y5xRIa$QZp zis3nKf8txiWrJ&%mvM~6MeUuvt)?s~l=k@&|6aoRm892*-~`H=LgYlYE( z{Cau;V+U1lYOV4;zITYT(_>rlIvZcp6fFN1>EvHN$W>7>__uft7`|X+#I~^5`b|qq z+rVl*HGbp81Y`a0-h+y&>S~9BOy$zOxbHaZI%F`CEYGDaCX6_rU+|Ee&wY2Z>KR?!X|Q_BMxNwi&z^5H4EFTA0c;Wr(GI{-zN_9 zwy}R<|3kDF|8{M3n2O$z#?i>eI!cB71C#N(NAw7{Ul_(joF?)$d|bC(OYlxz^ap3A zz01kBUZtA<7+x0}DkSfSe%v}(%|>x20=W?+gl=5LkL1a)Yda4o(x{X%B%E{yimh4v z%{hu`JMvVr{KOto{~Hgd^+dOS@_ARD(E*jufE*+JGIe+T=G?a8i-w2E$ucf7^)`Y( z#&#BVF=q?*{8|nj__(Nkirs#E>Z~^rr#ijyGMM^dfbl5jv1mlC1QyS?Ve$Bz!`c1` zHm~)qO*)t9SoemiJQx#$;!X^NmR=gU9=+E1B>c!ji-=_YMzU;YuGxnxAq)CLi(12l zT-xJmG&zFugok1F($5CT*N-UIF;i2s#I>Rv^k?YgeAdy90$h@W$$6C9c)E`?H+}3S zE06v((eM_nW_Q@`71u`@CkT|>-D&PAe@XLBb;EhY?cY1e;_~UqzSigHxIW;@TD%O4 zXj513iN83T*jX8JoTz-xUUngyjA*O=GF4yyORKH^(ogV?{jP6rTn5!hQrC87w^C6& z4d+pT3qr!R+>zzVF^T4~<>XbVPwsq(1@@s!)M{42A%3NYM`n&qq5*kT^;V^H=1y$U zqexn>U9q)6y$A6!cnG1QSXuOo8iqYF7haMX5pSw{%62=|Z@O9z&d)T$m*?Dq#5gy{ z4=De$Ph|1=56JxL=~Exf-}oy*xV}4LNrloYEdtUCqSoEi=>!N()Ds%i!MAa>| z*1$B2uFu1FGCX6*{U--ZqB)FtD7t5u2F ziAw8h7^4^Be%d^t5V)7W6K~b2lgL`je+ayao9;sFCfqX+s&-k!Wvy#mN=ZCb z4%lq**cI-pdQ_t}pihByG-)5v6W14kK~UC}lPP>$JpX9NmXj~b_(SdPKZXnXjt9P+coS&s#w0D!_>6#}rV$0Zv_FS!Mo}UGqb2_=PYUPX1Wh(bl z8X?B%4Op^|t{U!zJ^oy!H9SulNn>xyYJ`0f-u7DLPdc*~@68-8xkI#B5{IQtEWq`-MEFTi=l}x3I zG1IpRtb`BeUPoAIBs@QxabmjU8m(i0YK}gUDKyn#*_5r%CQ*BW72$0VZAX96XCy+H zkb8ojMQ6hzH+A=TmGMJW#Ap9<(oz-7KW4MG+E-;C8Bg#w1%|AOPY{28F9&qIQ0;4F zEFim}7;EBOb4t6k#z4g<42bQC)k><+eENzxnwsLqrZ9S|r?%{cMK!K`Bx-nrt?#j0 zy^iVQAcZhXW25^v@|nVlLq9TbpQj&@zR2tkD*xuVG0}QdEru}}>0j-E+a9zb|A$BW zNZ2p^Sy_T$P239?ySdEUbi=}UZI@>(#VpNXHilL2I+JepPF7F<6dSL%2w!}?hP9yf zfjq3z4VOx`BKQ*J<9F1nCpu@H*VFTpK%nKg1&g{QDC0qR;C(tl;s%DSTW47W? z8I(m;FKtn{4c0Oj7C*%}7|i_O{e;K-qkmkmCpyN-K;ew1clLCc&w#7r9KW^j#c%H| znPSCVd6VC~)2Im>CKZ1#*q-0=`I9_FzN;}Gb*Ofiz?gN)c13ajz^panaIFo;WsO0D zfI8!7>+knMb}{3UG`YYl>rH*^#&>p_9Q+HPOmO7ARHa6gj3SG$G$Tpo6MMuvUu?6o7;Q5MZ~O4foFJv-Imcg{ zKV|3%#SFb3Kg?0ks>JR%?XhY(W|ndN{pYjyvYcY*{q3gS?5Oso!H8TU)|VNteERA8 zgR0;5j2&MLGgD{mWXu`OlD@Y75_`(GA+q*5p=cFhc3xpcY*OdsgGWmxSvhAsKSKKZ zPup!f>7+s4*6~yT0Md@5u-|{uq?4Fsn2i3i(=Lf(LueF@njk=> zq;{Eq_v7L!ZP`^sU~;C|S=qG;79%ko1Kg#(>T#FK?cR(6M-iMcH;WmKP*W$TQlEXzMt z8=pQ>omQeJI2s`RvwQsQdV0N)^^}_>W-CFCOXe_p;FxZ+q9kwXg8eh&&9%C>Xww_| zMKgBUy&c9Pgc^62Z2TR+I>eiNa#MeGYdl9+C zA(;Cjp}msyO}NT^j0l&tjKoJ<<+L3 zv53I4_Qz1tS5CqEsfi;?7jh}OsiFF^zgmKSzctU$mXS0n)nveRe))2?syAW5)E={u zTV*Iaj8xF0POOw<+9+8r>1W*heoLs@Q#vEpmIR4FhlACM%m&*-!8)a4pP1j1OfE)} zy`P%oqjh?*%vQyp$Kh8sn^CJBp&8-zyPVEzi7r&34i_T6zP3ZW?khVlH9=`()p+`t zIGoDp!>cC(U2bEkJ~5mu1zV{R=cx@g0hx=QosU{1Cg=R7($e_mdMx`{XggR+O|GQ7 zqiE1233z!GWb3Ej7VgAxPxcH9U(y_l^xNGh3Lu-VeT~(VmE6d;lpFK&6>k0oj&!JC zdYtNPTV5F5z|TxZF+sOq%>nE=GajE*CY-sleFw)8gq*!XG3~EnxVP+5FI*1AJtU$# zm!srD9%c+W6MNC17qE)8$vD*O%BHqfPKTXrR(94>bY)&>bR0V=#Z%mQK4g2pXN$D` zX4k_r#>c_jZ6-9vgGaNoLyP(>;&NOAO$lS?V(IJ;!^Ou~;%(#VBGSqO*itQ)4w7t6 zY9A@Qryg^kn>@I2=kbFNV@IR?hk?|#;Zyw~9hD;@Uqw+ld$AD;gHW0J?C(_AA5xc7 z-`g?b2|~rFeh@h)PIKhx@k}YtRKFwQ(zU;ukofZQUQ8=_SXzbT1dTt9hes&pk%3NS zjgQ{sEryh8Qnj?4UO1H zMu9VZfkU{Za{qU0%M6SCrp79p_Pi+Nz3COEMU|OuqHe)6Q&)VYeR~^f?%;c_VVPbx zB$cQsGo-TRG%GuU5`qz`w=xYk(%%G2au%v;HPv5MVn3(YDJT=YJVC#`f;XxRGwf0wIY}*OWcmJ#FieBM7 zmo`OC+Yt~W{U*Rza99tIVl;0)Kf0QJGwtUn%R7)sf^1Zoe0qy&#AXq zTmCm=@#iP{WgA=T6Xy3*$u`zWo=H%I8sDT|nL5|ueU)C{YJfJlPNyNnmgP3PC}m8c zgoxZb!q-6T*`cu|IQoi)p~}dkkI@q8AV#sLWK)G=EPobp>2oLN#mG`uIrY8THhZCf z8mMC16dgm}miGQ|Q^PEV+34X)KQm{P%_cShqW({dmWEZ5cjUWa_Zv~gr*c2Ct~CD^ zXt^G=HC?@E^ODvtRSs;q9*!^a{0sJ-wfHpJDw|1I50|FF*ZRPc&SI+a{!aksry%Dl@fr`kgFbSm`<>lk8; zNGqyx;&8oMZd3GZ&izLR0=5$@;@CV?2dDI<*iT8U_slCoH3zr(!n=9t@bR9gNbQW= zGZ>}dJRH=_6Qpa9uAEcIW+^qB;Z018Vyv*zN9*D}@th~CbGITz_m}8+HahZ&qO?>l z!+5*7i!9r<%M{ZNmn%+enJYU{{L+h9glFl>z48R{@~|kU+*d5g&S2q(l5c)){G3$g zm~Up>?Va0-kqgZ;vcwm)MTQ9dpR3#w_%rmGUqwd>SIe-=yHg#~hu_(Jl|(A_IdiIC z5(Y_rEC|z4u`s`Efde;g8p_DqNHM;Wj|TSk67ob!_uGFpL;oY5*`lUdD{tuP>sv2&;);@osST~!a%3sw2dU@N zNm(L4OzC3K8~c=u%x7Uiw>s@;d;8bWP?+<<>f&Skm~YK#+y?BfRPlRO^7eR0`9I3XL>1{&HB*#w=}l+8_JP@RSuFjTKCE&zYCE~gcQh;&<*wco$L*%d+a%33WxKE6AaeJHPjR;@~Q;o50xq>za2}LAnlE1Z;I!3IsW5?$!52Tb8&i5?1X&|nLx1np7D^x|L>b&G8FAOi@kjl ze*y+QX1a^nTPPe%+5CDA_`Rw%4PW=Z)*Rc4uxzlaq%B@Y4k5v{bF(Lg7118@oQTIY zs-S@0`Eb4CWDg-yGc_02Ag$ET@kxdZx7VC1%egwmMn2@{)RKTOUfg4}zpKe9MZVad zE_n>&WGNlu8mh)TF84@(Fd6a1ao9a#F@3~pF>`ZQgT=T%@mhlW=}Wb0donJr*w;F( znoNY(9x>?NR#H-8GM|cS4SZ~KXgU*E?0jH6ny=9Z0Vq7*8W_!Hr5&$lzdHPNG*?B3 zA=>fs1d*wbPxAKdTjairMA9ln+H8?EGMkL{SJ|0dzj4FqbS3?3U|?wwr}I7~a%J$S z_vg>MvsagqQBjZ5M57yLYF&(`Dqe5=F1_)Dh9>YUHpRn-5Ba=q-HYOJt75iNtZ{q| zyQ;H3URYu}5lUBg>{(^E_7E3WPi-t$rJVlJUDaxP8jTBmJ-yd%r?!oajTurYx2CJ? z=0|g5DcH2NwU;N$>EN};8{=5iosheo?kCx;jqWcGc)e1{!%>_)YCRcI z(7;vBYkU+apx+(!Hbo#X1D1zPF8YLOm#44l&v!9Y%1pH`&re3}Hl`}W4C0+Wd!b=6=yyLlJy>hY zlFQDSuRq)5{9SGloS$z|#|F660eLuHVuX!Pr@k;B%qx&TrcAp{g4Dr9MRXLb)!s+O}A<3pTRaej`~Tf#;<4U~McI zM*HqK(IQ-+%)8W<_~kd(Y$^#)IQO0HAsgn7FG^<5o?hGn$Lp=A^>vE*{JF&>Ez2{g zBwIQ*PT30i`okGgH__1M=jIw=s(r9L3|B8%Z{G3a~%EE%T za4KK=&`vjF-=C$W&&9=z7PSX>XGdG*?6b-I{*SnwnY6UDpvTY}L@dI`cm4J2*EuTX zLHa#EUhB3$*mve8At7Nh`(5(u(P)lR6X#ZqS9&_dNS56Emd6z*BvVO{#o*67#2mIL z*6we)_PQB!l!{xOGC%C8a)TPQk;@SN9T?DcF9G5%VX#2|d{8|i#G0?xIiEC{P@LCRW&Y$=O8LTd0(!PEjLda~a z$a{8rI=8U!9yT-ll|nCTZFzb5!4-61Uc7jLQ=D-!t(K%;91ZFIO)Q25;>}MvmmI2L zP7ZZ!Z0v3~K0ZD&bAD_MFGCHhuwFzjH6AH4xma0Qf$j{21ov=`5~IY#=Jc}I52l8$ zE`KQPSdSjHM>85Mk7O&<=^5ziTAm%6#T33EC)cKb)W2fuH0pAGoFHL2r7~+|WaJaE zxXHHhGLnXXfMBY|DGV?nalr6nZ1apElQOojN+5K`-cM9JA5g`|pJ+T(&i~9>e-ZMb zxF6+N3hiQS%b?MYr2{$3(`XTet%9>=J=NF)!%{*fLfi)C?A^9Sm78etG*(;`67dUA zG*Im8pNx!*;8IXflJ?iLftdgJ=_M{Jf)QUrs0S!tp6K}np%_07(H1#K?m zfPHvsRkn9LJUn7vFF7wo8z`0<;{vcP6?sQaClxrmBy<0cFe*Fr# zVG?S$<&2>%q4rybR>fLJ9pLNUV-{*_Zs5ljENqb7>AzYi7E$$iiKUUnP>XShn^W!Q8OKh^vQ=jGS1z1q z4|pYyyZaS*TwS`JEJnuidc*_YP7{yogrXVb>nm7@E}1Hf8zAs}XJ-dhe13lZovp2H zQdgx7;=a4P`|Y!?Y=G#w0bwHBfG<{(Up~?YW&e5nR0lq#Y!P_&CBB)g7$}W%+ z0#acu`Y9Xy|>hpqga^ytIG=o zfJP5AfNGs90gSja-zFp`ma5hC8HqQ29+zS3>!$aKIhubko2)zgcyk}94O+hypB9hj znsAnJgKz=_5eeA?NkPf&7rMu=1GBhK%F_+ppx=-QS&>fCHi)qr=fnh!u<<^0eP-@A zW-??p=6Y30ZzcJv^}sAB+nEN(i?T25iiG9Sv!h&J2h=@di;Czft$A_FT8c0TkcuxYf4j^Dm>t{GS zIl&?!3F^>I28c!#KLTnplqUArd4KsW2qBA_om&DaSb+jfKpxm^mmeIgj?4q?*9JUb zvY4sqKk#%zp2;=QA534%%q9o373e@o_J&FDbgT@e`^N%_dcSgVdA4=u-n~>PR-*A- z(f$3Bqj_p3dlB(ZJS5`R2BKCl+?S>Y)C}>I?*$433gBw!v;6m6fIS56?dhw{F7(_L zv8(%bQJ1qCj%6+>)+g28fGOHARU!`CF928rHBL6-v1|>&yq8g14V7~RN6sRXP-FS) z>h|0=nV39wWTDp8?+lv3Ere$=fBpHEV&pJ?{tGFSt>=q+s9{Tr;T)|*5&9S znyPwhFsX#0A?18qFx{3UN)(&bGj(-!(HNH8ns->B>L0M%E`K|aSR8lpDi=t3D}_U= zTJZ+>PL2mv+n@Oc7o94rxnPsn<9WgzR%t8yQEb&>s=zhb)UtXsQtB!3^~J~3QB!B1 zj%C+?4?5bc>DSSv3&&f69v(ikS-WZ0?8&-ZEpsm${@}HG8|;x_`qMJ ztHbxbQkvb%Zu`$$7K@qg%ZszGK|%VUgivaZW?YcM1T+#-VS>Ogj5AmnOg)D*OvMU< z^*wzbO%;CA1Rer*XUjnx$LX-t{1qDoege&dJa*^Wc&Jh;h>3a+Ub>oXJ|l0SM{x5b zO_;1<_wcanau%o)oE-|!`Sv@QE@;GL&_g-OrCCh!bpIaBE77ZD00>0N2poPbp!j!* z*uII!ar|9dQ;E>}&l0j2-25y-qN&!q6Nd}?1P5apmlEc(P_%``O#O4Z3Mhvd6(LaO zdst_YcTY4lG?sefOlo5*Y*(bGrlxLVV$yYeL%#4Sk+>eh*34qIj#w!xLB%! z|BRR^O9jprah0g=tQT1%>c1r?TG?JoQ?XWd>AgiBGZ&@sbBmDwGWq^Ld3*nJWkLn$ z4@gLQ|2FwvQ>=AnzJYe18u7*Z0VetiFZ`+JS?YRXz0&Xffb7|Y?}J7xlFpmil;mM6 zNy-vVF0!33Z+^IX>RljFNudbXrMJ)?0&-ls+#IehJT@RcIzc21LBaI&@^ac~!LM}M zBS!)5Kb)r)!)~+GpDKd?gVmxJk>GLvjj1U$6xrtt454XaF@s6(&`^NpHm^?-$TY<(XPe-i!S(yYt_rCPrg<#6XhLAg)w_k7-;8zJI?kpR%l>@t@h+GG6LLp2IyjVzvm=A!?Z2^{4ng4XBm@vL`!zSek69}uV`dHq^^F8PFj5`o16iY3 zEl3~>bYU23wW{xcTPkaYgFl6kJ}Xir;NrG;T%FBwb#VfU^PX?t2nh*kZf@S5c9_Lw z-%1sUe8g%_RBX_<1V{Vm?yWVBYxY|+J#c{0FfbH21ON@f2Veb-6pqOSsEh)GJ`&gi zi{jX7hb{f({-pJp+8BWRz0Ozpify&$yItLJoRKg`6YH1nt+=uvNaq?P^X`noGwg1s zOpFG-&C}DwwdxW8Th|ne_3u3w40^%F_6Q_UnnXgc1nKH za&a_^p*D-8cWVb3vQ#YQQy)OPuRs)V#05dF$W&S{a@CWYDV3WOgS6dW8A1a~7hZup zc;2h?;+}(XAh8zp7cAo9;#)OxE8q_x1YR{&uK5t14CjC zfr!k%TjuEGq;!=JHQ2Zmrtg>#cYCytRB?V0dVX?Ao%$rDVnGMYl)O&H?yH? zfHM9T7Z;aad;@9n)wxcbo&ZzUH87ap+Uf)A4_z%!+3c)&YP$Y5s4tLJos2xNV2MyI zkvDHpHYUpvF&1?%EeQoLcU`w?k1Xx%^!nntD=I1qpsct?-WFlVZ_sSFA@9~MTwrGd zVL=bBmp~w4xJ4z@yd$+%JR0Q!>qX(B;^Jsd2U_qyq1x@%MZ6@T8T5e5lttFH?-fBy zu8!sREp&u|Kq2GgjDdQL(x0!vi$|;4PU>;~RzTnutTaWX+}zR85vOtrtgDCk_y)BT zlMViOQ_^zmK=bm(eB0enG5HUL77FY;o4NW$jNuWNN+aY1ooUV>TA;m(LVY)&Ns z`od30Sb{P_%wUnPUONsJ0HiHI8vwqbv~;N5IfQN$Ou8+I_?;Avw6yeifz}N?|HZnl z?;#-#P}JJn+u!7m%`Yzc$gAX4);XdG0W4&+cQQ9fXUe+n^{`G=+4&>sbAi^o2x_J4 z3%2i{3j}C_rH}N7K``>1tut0fJjTL8$^igKy6qwvVBW`I21dWuxdnPa2izWzbJtl} z%lRTV=I5J0nj8av$<(OlfhXs{11^8}5L1O*><3cEflq-CnVbS?b3d9DbPotl2Cc@M zz#l(|iv9%cH_CVoIEwb^{)%WU+do_>GgATLwEj(F%rCjXaNrq<%ld010Kg^#$Lw{Z zQ{YnmdG&x#R`-yEW^$&^Z8C3-6?D3_z}z)mmKY=O!J#%nb*ZRn0`~s26AohTaAWe( z-Ota`4|aEr(biuXS6O>tMo{u1J;~_k=qOS0%Zn`!8IZ{ECY2d2WZMdpi@j(6_GFPB zhE8h$um~(d!cGMZH@QFe>X4+PAVKvR4*XB^b1_;h^nZi^<^R2%_rD;&^Fpc~3Z=A+ z3;?mdlkYR{j3-&LnS8LXdly=JF32ORh^)O3G{hm%&o!-xE^R%dlE<&jIl!dUu)B;7 z91ErkahpTjRGs#=w>3erj-KAWe_y{Nlni10YpYCI-Fv5>CH>bot0BQpnF)S~3r%4U^Y_H$3QvacS&8|+lIAUs5% z53ylfOlCBTDrO?_7|`%t;g~}IsYq3T->|%zni|1t?e}#ro*fnE?qFUQB=)sV51*H8 zk=~5Fje(>Bpti5Q0XG9m1Q(!C^XzN_EJiUs1_Tu39L19-9}N=RMo))-`C#Bfm2}F(2coM7DY`UIQ`1iU_yr=e!bq~G5|^ie?FrjpCrZhdU=fRt3? zXmc7ajZ;BC1zN+X?JEz^?h(F+42KDEoh*bQDOX<+)DnJZ7$U_vFc&b&s+YI{?!k+I z7w-&QaJ;Z~c({VA>pmu?LGu?Z{<3nY zvJ57p)N2SZV+X-Hy?g5hvZ(=ef7A;6V5AKKWXE_Us|Coex@Nb}NE`U#$7>o_onNq? zgPzrysd19cSLc>Ua9A6qsi~={fL7TjJ7f`pet665;}_(ly78(tj2&KnY-=H?mjNcbq&L zGI@2lI`Xe515W^ksH*8?&E46Sv@1J$;rl$?#ZzysDs#Y47i8M`BAo{)dwRqECsYb~ z-9T#ufUrY(1vEff_Q3v*H>4?~^y`e%dIBsk5TpsIa>*xpx32FUp z^cmH70$a8OB;>uIAoha?zr62Y?^3V*`g8+1L7-BWe;ZN3-Bnts(J!7*K7_slvK0kw zIBS!Wjh9<_YSmzvp+Q7b9}70+xd81=^R6OkIJfh(k6Y zEdTPsnTSHJIUP*Ed3UP6i?c)J#SCjWc?zk(NR`vr%jskwaQNWGroesLLP6D!b0kaDG{@pv75=nf6Nx>FIRuxFz^d8(2TI)a6j@t&(s9j3DL z>!4vIm!VWl4%;=#i0s*O0c(O|B;uC>uE}*YWm8dE`FDN28@L&4p+>x3?f#&MVX1P7 zG7UJLg(frD6ylcit@qTO7IBA-nZb5Jnr7v+TnfhrqGM;hxDQY15-tQFyPnCENI-;iEC!r_?fv5Kud}<*0rrn_xp@yH3DSP| zOHLeG`4SXFPD^_~PFxWDb+ggTT(gifHK{cKW=Ls}5`fmAcj928xXST#24` zR9i_Ur@@2roA~2qa?NmBxoF!J6>MM?TUlFovZ_HNi`jU%zsj zR#sb%IJ=tS;$r5t@=}@WNXh5`5rXVb^)Y?=^l4NB1(kUlDxcNywiaAX(2k=6p9U7S zX6uljPFY!xcTH7QbsL&*d+anc_xL+b77xKDF4k(g1w=?Hd*o+YT4;3iS9m81I73dW z&&#QOdLEOK1~+KFfh#bGd$(^>$~$cZDRKR<=4^u4SzIC7^3A<`epEd?EDQEqSb8wT zjeh)L3@5i_@@3;p1=9VovnzEskWlGSYlULOX88w2B!ar5I0(sGpne&T<(V<&x#)D!Pm*^3u{HaCOn>v@%PStRaO z-s!!%n7#UMz3=MkdVDr})e999L-g6^g}M~Z-26O-8k#2e+O(%fJ=9;tjzdK*KV|o~?t~Lpx)HyoP z7lS(!&-<7FWEy8@>-#^GJb3WN_dc;=nW@sJE?UFsvMCioR5x{q6QBS(5F9Y>w{PFx zSX$EK`IBk9qwcMNh7Yn~$Pk^w`W7CH{`KpZ^virl$Aiu3m_L6s!6zav;>9@>L3Za>nJFseOZprZVJpe{`lNcm$~od=@SI5`46!0uOI`x2L%KeSWNPr3?UBo6N6n1vzxH{?cST}wT zAb@n))5VDinG7{;PT}pvx*ZQ^+>X?MBO*&7Byz7!R(+zt9?WO&U`zuLjhOGB*MQJ^}TrRPW9Wg%2qk;P?2o z>J|=8{xbRmy6wb%lO7AbVP{v+4V27PW`-yhfVf=wUHS^(wUFJ+>dfEsGm;JLFH0E@ zXCSfNjJBH1tQ4l=y4`>lK*}2reu{F5VFQ3Eil&(hG>)D@c>yCr(ITP@*T!n~xdW8O z=UiMwVI8`)`B@oArn){^9*j&G*!@Cq)WI}@I~7Q?4=kr}P)^8p1Tre%na|0T;4%(^ zZbo+FfX2Y~U3x`ED%5RUtY+u$0wl+l_4T+~4te_T7w*!G{vJDZ?ciG**s zTmc=a=6Axm@<_~r8!4tXHMy5J62TFJh+^xjrNI0F|Bn>UU3B%TGf61)p#OneJA!@V z7kdbHjuylR3!T^euNx4e?tjcRLZeqTHD`;pf;?KJ{Cf=iOp*Qcvlp)PkGT~u$?i2&!`GMPXM zI%?`1z88w5Zu?kB#@Fb5XE;l)i?GjStF{dea!+iw+Q9;X_7hq@ribLIwpId6EBuVC z5$3wc*#b=)DyYV^4Y*ooy;xk zkh=Z0%KFNGya8Y$OW=%x53f9n_RRYMp1%(u8`3(xo*n}3qn1DSg`g7Bo&yI>wZiff za0V+Nio19DZ%cgwrw_^D04mk~F#UUbVLH-Ha1V0<+oqHA=F{dI!bm%TUvBfiS~-s6 zAO6Q3R`CD6rW}4IL^#7V8@-8=-^pD8f z;U!Geg@uZ=>hCMvi9rQ3Fxm)kaHd#02rkZhanL`olYBtMS}yI+fBUI0AKdMJ@1u3O z!7A#Dsl#rE-nH0exrQffOeHLED@v3d+o&`{RaH_h>*mZ%Mz(_BbmFz&+m=j7XYv}{ zJM}=$^ZS?QeVR&hbM&+tK}LNfAB3-`V1?(?tvVk_KEBItkfW+*Ga3^g6dLMD_8P0J z#XrhntF^|1x3$z{K;V6%%M!sbvoRYr3*3*mgY1(o3J3`|mY5U6@1-7MH+0-FTN^cu zVHI3lxuZ;D$do9LxQl`Z>u2%m_+A|FT#%la?W56wv8c;oB2e@V8Hq*ecVM4=LLNeq z0fW}bJZ8~SPt!w!ugPPODEFu5gT2Hf9h8gN4GQ?P`Q=Kfsy3$w>{dK?(i7-OxIP@l zvPT6IzvH#1&f>6>G)m3J=KD^wVh3F;`Q$F|iHWHD#Em7EPnWPMBo(qh*O@4+ca+`y z@4EoQZ{Jdq4<#Hahkq=Sp&0)3=~1~^Q;7>}XB_A9XG}qClKakXg^}sfnZiW=59S8Q z^NpbKN*s1)DCylJgT7YRbGB7N=6;yWZ_SVE>#-E5TL$gD0vlTi#ob_Hka7wfrYC6g zxUx|=Ve@@qcxC|mrPsRKMjC}j+iZPEP33jX%pWhKrdFz`h^UGryqpzID}aDNu{XY@ z!t&`Um7JuHH=8Y70Fruu+-tIAO5s9UEGi)(_9IndZuGo7!_|>@iyLt^OIbJ>S+Z|s z@=4M-9q38f`3`HGmea+;9?$wb(EArP(exJMAB!C5DAB3t=!QqKJ`H@)X2N67FZ5%L z5Z*+|kdf)1uF)Ig7#Yb*@(U6Wl4vV3z+Afbxc@$}v&JIyD_xKYM`;&PQ4y=~I!ZKg zDCzJ}Aa&sU#QrdODPNsf?BWz9N+RL&s~!8E^g%;O7Z;s6`+F^h%@y`EtG#iH+)8|3 zq>T58&#N(uNN)w76k0NP@XMEfuban^6+Lj3cXPY<-`R0R!h6L zpI$!>l#pYxbJ5{(dN^{pk%686TeRqVcXupnJ;GgFcco4vLv>~g~Gaog;LN`(Dlv3mZwyB?Dg z@~cBzet+8x`q=CDiZwjco@ride{vn~BfpTIUq1<{!W?UTNNcP6kuDdli>YWtn~lKP zd%e_k-f`Nt)<7AtCM_dZReLV-m1(RTN70=fE3!td%wf8=G%?}vvWI;<6m6L@2BY*W zEG86=CZ?vV`hz+;Md%p#I~~_FwM?1NpXjXjicd^W7a7c~l9NPoGcec7843rhQgf7=#B*>f~B#>&j^Q1$iBM8lsPshsA+qYf%cNmpy> z0oIvsXN!^Hyj;|;9)=M6cZ!e8@D^s|e0Y!A)MQ=1UzBV?Bn{8We-hj=$&cf9#=R?S zv;NQ`I8e-N`dgy`%Ey)`a6pe-Y;8Ad^TZ#w6iu5~j;cTFkT~Lv2XRSqaHd z6g}L((tiE&VKll@Ln67l6MtdDTcNUD+RFE3liXX&;V+c1v5S__-BC1eU&DB_^#0l4 zgn7~VrgBd6at1j%hF%wuuRWxDc50XH?9z`!)f3k$~XISi42#=qIg_$~= zTw;#3!lJ(A(I)l!1t|l=hkvhsjj12aZ4I<`!P3*W0dgGuioQp6~M?*c9yU z>f+-3ys$!fB6pJ-rqg-*o17{aZV}#X#k@=RiTQcyCr@zkUs-jK4Q4uow!QNAmt%h} z+w)zqT)fvIxTURaaeYgp;_%#cf}cMCx#OeoN333#E-o(3IJJkXY6=>raA?k;G<%*~ z!uswms)hlb25sBf3`KT{Y$(&=>D$HNV7g)nkc#&WVQXA* zASnDkrD%6=ajH!0wBJ`ZL2p=iaJUfDO|sW)Qg;#Y3M&x#P^S$CzAJBbj_LmnFg?td z*=6#|g?#epN$JPsSGhAD9{Q{J^%|%1af7+#?dqk%MLDy~B{`W0^IHDjl`^{$f$9gB zXN**rTUYAaZq(^A@ZqdXEmCR}f+j+0l<4^b;kD6@nQ{q2g6Q(h*~P&SkJB=)?5xGv zGO;X0+KYVN&>u0nS4&@P=cePv6h=Ng9tA}%b0LC$S+8NToluv1FV9E9$X+TW$8Ie& zAN8XsGDs-}ZHni3FIGISb*)$$j%*kF-t>55hv_QRHRI4Ddt13!AS?T9+FhA8@_S?? z(FmdAi5~IUu7=6Rc0sK9Q6p;j_jY4rjy4HYl)dMg*Az>>r}eX#ezRK)AIy{~LR6Ub zh_*O6WpUdHn>oy+HUv-q#AS7pJ&}{Mu9z;`O=}Dz4i+;L|2>pauHy0Yl0v1l^{=EA zy8lYB50YSy8% zoRrc_33YX2<_gs!`_1oiYHqy=XefAmzpfWh8foa2>-esFp`|LQhe$idQP4@)eflUL z@tu+=km_{P4RtS8+-Y}L`ghRNj-s@*TMp|Vvi0+x-7HXq_(uo|eI=^4?;s%#zc^ic zSx?=!B1$LP(_m~2Us;Kim9`eYKzWljj)w8;WT^ki+Ner;-d!$@(VP&wE}EjXwZ2)G z>lMpb1t`?QR57;kJM*@kGe!KPaSp;_V#TZvn0tzTrjaW-%)E5z!s}R3mF+<(cqEE- zovnV#d_twX6aA(x?`1{Kn7WGPUzgviv`PwUWNxQJFtA=~!L^qtQO!zdznlyzRb^xx zj%Mk&&vN#M5`=4O2NxD9?bhu@!o!PrcCn1wf-=SUd%oXAF?tH&fb(3&kSD=*ucs)# zPI)xggZg4Oa&WsKr||W}mO69AgRYhDYO+kFA1^vz>CIX;4V&4f`EjN1M*Y{0C>h8p zh+Lg{bk%KgC|T4zJ!{8Os;w7;@BTETxmVe4I-9s}9HKbVhbC-_cu4ZUiK&q6MyGrgT)KXPgt}Ecw4kW! z>0#j~a4#x*My=<52XWMK`SRro-mGMcfEjnrt1r(-hOYFdSt`Zq;QK97oCmlOos7dE zAlVcxAU(H|Y~$p?1DRmqkw`m(0{`(GBvR?ld3+>N;3+IA66w`qoV`W5ZY^SU@v1(F zbWdD5da>VudS<3# ze^8~UU8@}5XB|XglA#1&sEp68thl&C`T6Ui-TPT@5A}cf@~*^h znrG>?cMT0s4{Ur~^tEsxh_ZR}W^#O_g||kMc1%Neq-DlHCe31OY|MMI<3?SSFwr>G z*xH(ki`#j68pE#h*rJS6)DBcjd-HanuZdMUe?@X$o<3a7x2Vi|1})U3mZF`ccj?2$ z8zL!BpQ=DBdxtuwy4C`EcLV+*tEiYxCa-#iVvEhOc*Kq7>$r@c99PGapti^6osU-a@|?ok6!hv^ zxqQWnw4@|8%OC7!y^?C@#E}sfcXxL&{yUoF-#fDW*^Joj0^H)t%9dFEUS3{_bSwbl zu%LzM=n+jzP1VFb7((A)3iTEaWH1~H+9EuZQD>3g-`}6-F=&bcJ`IZFfKU7MBXlih zvf17_iQc@8W zogvmHIqCX4^ucK50uV?XnYRuL_YVx{IXXVeF1fQvB9bne*-S5w#XYwgM+=0xT4;tVb7+I z(JTKFGyc`55bXZzAoI6AAipx}L3DIJiWxbX`4mcOLPED9F=l5E{N!u#ghk`KcM|V2 zs8kaegPJLIFi$@P?Hotppe^c{QZq6PU0p|?+9TRITNefDe{f?Q6Uzr_(LZyh^J#m< zz*v9N$qS98rGY2|%2u72Pcm-B9~lLORI8m)gSrvRXziH|4Z6=PiWQg%YeQCZq~TV< z`^TfCioQtS2@W36vSgDV1M&2nT6RGKI27bk!1B8JV+1 zVO*`Kka0tmX5!2PloLwbycfQj!y)m8ptm|I-(d5_syw-8YHG@}c!689`K6e8kF#eF zlVM}-=z}d@2$j}-W~@1_q{IS(aTd|BIoOih;~TJmbbx#Ph7FC#Il1OXw6h%dJsW@X z=8ZS{eRk`=nF?R+kadpOtAd_?aD+!h*q7bnEkvE|SXZb{ZA(+1TmRjCU+;++=P^9S^4pZe8dBElosv-sObTOm9XiWAT8?zRTWq+Er=ha)H(1Ewz8Q&$NA~aE?@HMr zArZzL)Smr1>?xz9^k`V&azb)(v3Xov+~}k4)z#Jdy1Ku=TSC(43$~IJmFLZRVUc<3 z_HFl1ua`)osDb?MpHaod=^Jc>!)msH86`T6W8UHxp^`;o;%dRB}xKhmU zo=CP9UCOKqCUu1&HavOqBv0G_(4j*}sw19?>(ZA!&k|X+1eYMQT zd^|NXQ|IPkmCFYXz-G&l5Kkz16DTW(=G1hNiQ}}SPF)k|Ate>K)s_M_$45lGSod@=dyc5BwDtBQhk~$Yq=7*KYSqpzevp}IwKMtm-&Lk&{52YG@rt{_)jp=JT|MYg$Gn8i@%r_z$dFym zawsj6MnWN8@8ttlFH0D*ejmS0&r~V(I244LeVdC#1?~RRFJ7fnt!!=8kpRz}IRo&g zuB9lbqomy}BDa5eheuI(Y;0_HVc=1d8ZQO@JKIClYaZ}Tcq-Zy_wQMCv&OV*5Svb3 zJ_Q)lSYpow%MkUmP1!GAD0X*ui>$DYz91THC zQ&Us7QvGht;kXmUK%rQyWNQ;_2?MN>lP6EQWTqlwhp}CX+EUZf^e$a0P}sN6 zz}Pssu&}Vpp=W@Zlis!G2ujovF_Q=!QsW~~iNC;ISz%L2a)tcC) zXN&qy?pCJy$69%K)8F*En+~j(`37 zT=Rv20xsTG;4P`i$?8fgJqn@?J$;m!Z3aqf#JoG0^2dSk7ND*#8bq{gvbWE%^2G11`e}i?Ybq>rQ<^WmL0lvsoOkxS=g7l-RA!C0m%3H3qv$ zr01U09PDAP42+NnVP1~QCyzQ={!vHb;}l7qUHC(1qk6Zvy5xG(Tu*MeQ%Jb z$50c{mqV9gOin&8VI}P7L)TIyqwigX^6bU7fFCmm+4dli4i0H3$cVdpmw*oDu|K}| zjG=D80Rb5BvLWWu#}wF?eFTxfbs~0HB6XcEX7}G08sf5?yz*)IdCAn93yJz$NmhLh z4i0+4A;*y@DO%LW8|0i1?EUC@rZqELf^J-StD;jZ|908wr3DLX*M`; z<L!*_OgDWYH ze*Jo}(uVs;8z*-CPe5MDAJgAEm1pDpM?zzJ(cNxS+dk;t}>VHR^UQ_FazjZG%fL^iln5aX{fg?Dw2#8}c`TtNt9s zo7&|=T2u5_D2Ssw-lDuTvPQ*#bnaMyhPb#m-9_|%xU1f;l9H0%T_I{R;J}X_3Hr?p zSn5n|e|5i_P6r%Fu$6h>`_*!`+mdzXg6|<^c6~5HG|$b$=Qb>oI?~~63|MAwZ?9?R zNdrH4X?48AS&o@7zcPoK_=5l}?d_SUW7~aqhp4>l6;TbS2~B(XnYA?nGP3P4&HIRV7v1{fQ)ttABoMj(v{Vq*l+|N7Mlpo8G$ zva+%mrB=$2y2#Uv45@3cPEp&Oo|)!rV(p4e#N55#a|caKCR)od5{N$YE|)%Km%Qyh zT#AcUISLWr6%D$zEngF(oiy*B!|O`U&2?=WzHhXwu+0fHs8(Ut^2eCv7WyM=NHyN5 zbHeY0WlxZ#H?6u!@H9KlbfnFh?>)*!XsKK%KEFnM`}PJ@FRU38n!$sr#+@+t?n`(O z6$Nv#27ZwCG1cWr%=|yMT;7no0A|DoR4(T zw(uQRC3GOcV4JGtS5Mo4DTRm4o^C8JKLRpz?y1PeIPgZdGG3$jF@7dRR!%PY`t^l? z75G9VSlqjE%oELBX`G{Z2!v#r6n~-itMn`uWuDK_6TV6*k?u6*7nNS1$uy8dpsr!@2ip~=v zt29!E+$78X2ulTgMtPnhQ*`Z0?2O=wo@3uiui3Kia`j26h+1tjl00X>FGOAaf!~c) zNsRlx{u$#*1u_2qPfAwZe7(-$skX9`WqVS)YFVG#7Mi-`yGwy>Nt);XHb{kcxBU;UikR&0c>@u_AZ{KRf8UM;+94@ zIBLYux#683>vZ<}^iG?BBK#J(PLAU~`52+4*SbchbP2q}0Ud%sj5;jM z_@oq2^Q3QMll0MhKoWHOnQ^wTy!7fjN5*RR4JAW*i?B|C02!iw*wN9ksyKB!pld&U z&?7A)LmLkb-IG}Q*kR<%n9K|G95Qol@zm#N;6{6}nG8C?2#zh%Ok$(}2|{=jQOhxt zio{jA-4sFx6V)0c=E0QVgg9f0oB^&p^kQH!UJ^E0xuc8o2_g<=X2y<2L2K62W2E=s zqc+!4^{2`4WHPxcnPK*MV4t#bz(@CUTw{oH4q1tZ3@==`kW+rYqn|ZSy$hLL^rtjj zix-s?cp7gT=Q}t9yuU z@GFTF{{(Y@(#a|$-hKV-rj~OMow4dvAOa%| zjViLqfCh2hV_KpMHoC&;`0vI0e*@V4RQvz``GBkS=$p=A&pqve&?q;refc-6j-P|4 ztCwPTu5Ke-h{&5CPAghvdu@Y0E1NiadpPF9nd~S?%}`k(x=PSlC!v)-+|8sQ8RFaE zRrL^D(?LhZCMLuY2>RR}e8m%#MtLe`;0r&9BeYu(GNjtXB8ZWd>&#REpb01F?~0@6 zt2oouMT8O8amLQ*h(9n~D=mN~l7f>-fn9pK{WwUa*XWblU)SV*{(NrAx8!^zI7CiR zp!MZzfW5F>B$SlvQu8P;X4;>QAubwn@^0qvUhP4JdClcLf;Q$z8Jan6S#>Y z*C@`S(S869N)wvw$5#CKwu1ekM=1&C!yTDFe|}-d_4Ta$Ouu77QmophK1Y?S^uC(j#xpAMDH@scS4e4??D6*eH$OS8bdCF78F= zf8*{t#W)m3_TNEi2y+nH{M@I8e0%nRRTiR2gLQ3(??D1Z@_qLS3JOvu6GsxD;!zXL z|I`fHtT*vyE-w;1F2TnDp_BofSnu@wphf0{z>g&eJ6xCRR}u^vhz%vO1meNUGDRij z$RzHWjPR^QR+^yJ*?~gf5T7stW830EiNSWcDYyft^!g3?#LfbMAEM0+UKm4Po-La4 zwr|_^7F2(XTIZ)tsMR8Nkg(Y`HcD~uevp{jTGXlVUq?x?OTvLcPL&O4LbfKtBLDD%*3N7 z5ZrNANC2kFy`Z~R5W%$xfqE9xY>M>3E?uPbIMcUVyz00C1e){Fv9X!p2FMD%AOZ@` ze*{XQ+qMgP<07H*z-uDrF5JlCzTzHeTPT#(A_E~BfI%u68W}};_~8^df_`OYDtF2b zdzx8JRU5HzxScJMG87IZSORWB^m-8x46zQ_r)x+sSPkFj7ZPelS84h;`O#h2P>$tm zsO@NfE=1}gaG&yYt}?@j#x|H~YoCb`Nx?~ZEl$wjfmaEI79-6lVcXAO9Vy1q+P{C^ zyB%0J(|=4{UciR?IdT^sX~)zwPK?S;W&;-ju|_Pr${>n?vF(iN6i0)IwGFO}LD4CL zd9zZxdHMJVzJR0(luwp%*w0Tq*pZ~g%+^+%KA?mo&&$gzn)E(#KRTWiJik4Lu#Fzx zXYJF)?Ru)J*Kn*x3K139!@&XRkbVd_J`zcLht8xt{~7ORCb>t@-uJk?ygW=@+7A~t_YYs1Z1euP=Hm09gw2qr@3N3>mC8SfiF*X#4x^HwUDl*OiX#Y)bPlR>Dq(HD%r8CX9JEH@r2Nv25Yh%t!R~e&e~Z zz#gGaXh3%DJ6Fa2dWn{X)3N$sI9gD&5TI=!V`P$9ttx{NAD+3ms6w75(1At+cLf^ARlCN~end`ORXb()ZLOu zZZ4`@T)6&+#{Rz{dAV8_AsstAwKAE6%sOY=Nmma|q{YV{#w%};vOY*4H5X%0L^v5B z>J%*v=080&2s-B9HlZ?&@W0ltXUzFuODz{5iX`MEnv*yg%Z(@~{&^7ESZM=@8r=XyKHU98>TZeP+{vj`& zk%^9uQ|^~85m^H77+T#41km`DfOa1rA5je?UIZYr2spO8o0~0ovWr|zP<@T!3S{V7 z)10?hXiN(4;srr3ny126a}dq6ge`@e=9n+d5;-iqau7XZ&jJajhB&|g_kcH5dKBt` zDNf{iI-lrW{_H#WuPho74IJ_K4o7u@a>7GO(UPKyOnrwh5_C={X;Lx$Q5 z($U&$kA7sVQh8a$#b(Kn0!0&O&-1dgPb4HH+~2mO)I?yvH}ve^bJFEs9sjSHwaYQp z`S8Y(Ji)Fs^m>-lbAke@9w@Dt0-N4HnosA2vy8!YO-y4wMGWc&if$4N-YS&j(ty1| z?)uYCAwBFfn`JhGM?o$EbxH;;Sa(-Aq%ys4_MhfS&uCSLvn^&GJPm3*f=K_@uZA$6 z=*g2!#Op!FDTYr7 zFJp!btSWE`krWEW5Y{iAj=)yvW6%c|L;wdJHzViaG?L8@(o2j@ShEu}%O6v*KT>{@ zX^ABahpYzTATiWI_Jq{+oMa{AZ!M(^Bx%T-#2gHJ!iFNBvNr{B*51m;$G4^V3@9c+ z6VVM(kMoR9Ldw}D7(A!OK_eAbZW;O|D~Hf~(aWw!p_3wl5P#t*^aic|((-pN$d&#bz3u=qdTfeQC3U}Mu3xuMd&$%YJvv3 zF54)uDyYN6&_<_NC^sRnic^7)p)z(7vZ@~VXncy@Yg-wG9-dAr(9m%mow{|K=-nr4NW);JBhNMGHtSw1NH_{kP%gV*ZrK;&qo*bO36`NnG`h`^rSZDwY=j+S9NW{9* zd7qaa-CaP|NSL|y+gxy@kL?#`EIGUfLPnR*F2{@a=m z^DO5{kD+Za=2)_HpgKM%MB6?r)lCK1+b zNu-zB`;JUhl;hjk#x=I>L%ZL(%5{YvOJt2;V&tH5CK0b4rDmnOAqX9j!e4vH}Ja5mZ%Gm1p^CG4ZPH&S`T= z&+*aH8oPr(t*0n?$w~r6lnTc7-W&h` literal 0 HcmV?d00001 diff --git a/rqt_joint_trajectory_controller/doc/userdoc.rst b/rqt_joint_trajectory_controller/doc/userdoc.rst new file mode 100644 index 0000000000..197a7afb30 --- /dev/null +++ b/rqt_joint_trajectory_controller/doc/userdoc.rst @@ -0,0 +1,12 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/rqt_joint_trajectory_controller/doc/userdoc.rst + +.. _rqt_joint_trajectory_controller_userdoc: + +rqt_joint_trajectory_controller +=============================== + +rqt_joint_trajectory_controller is a GUI plugin for rqt that allows to command a joint_trajectory_controller. + +.. image:: rqt_joint_trajectory_controller.png + :width: 400 + :alt: rqt_joint_trajectory_controller From d915497394710481a98cbaf7b6db8ef9a368aa3e Mon Sep 17 00:00:00 2001 From: Franz Rammerstorfer <60260174+franzrammerstorfer@users.noreply.github.com> Date: Sat, 30 Dec 2023 21:25:35 +0100 Subject: [PATCH 421/524] Fix ackermann steering odometry (#921) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix Ackermann steering odometry bug issue #878 --------- Co-authored-by: Christoph Fröhlich --- steering_controllers_library/CMakeLists.txt | 3 + .../src/steering_odometry.cpp | 4 +- .../test/test_steering_odometry.cpp | 110 ++++++++++++++++++ 3 files changed, 115 insertions(+), 2 deletions(-) create mode 100644 steering_controllers_library/test/test_steering_odometry.cpp diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index b64ea204a8..63de42cf69 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -67,6 +67,9 @@ if(BUILD_TESTING) controller_interface hardware_interface ) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) + target_link_libraries(test_steering_odometry steering_controllers_library) + endif() install( diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index bf254bfcfa..e2ced036ff 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -270,8 +270,8 @@ std::tuple, std::vector> SteeringOdometry::get_comma double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); double denominator_second_member = wheel_track_ * std::sin(alpha); - double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp new file mode 100644 index 0000000000..173c76baef --- /dev/null +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2023, Virtual Vehicle Research GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" + +#include "steering_controllers_library/steering_odometry.hpp" + +TEST(TestSteeringOdometry, initialize) +{ + EXPECT_NO_THROW(steering_odometry::SteeringOdometry()); + + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 3.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + EXPECT_DOUBLE_EQ(odom.get_heading(), 0.); + EXPECT_DOUBLE_EQ(odom.get_x(), 0.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(2., 0., 0.5); + EXPECT_DOUBLE_EQ(odom.get_linear(), 2.); + EXPECT_DOUBLE_EQ(odom.get_x(), 1.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), 1.); + + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., -1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_LT(odom.get_y(), 0); // neg y ie. right +} + +TEST(TestSteeringOdometry, ackermann_back_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0.); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], cmd1[1]); // no steering + EXPECT_EQ(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner) + EXPECT_GT(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); +} From f14396cc14d023a8b9d6bc9a38331e4af59f181e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 2 Jan 2024 16:14:10 +0100 Subject: [PATCH 422/524] [JTC] Cleanup includes (#943) --- .../joint_trajectory_controller.hpp | 18 +++--------------- .../joint_trajectory_controller/tolerances.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 8 +------- .../test/test_trajectory_controller.cpp | 5 ----- .../test/test_trajectory_controller_utils.hpp | 1 - 5 files changed, 4 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index d16e4f8267..0ed2ce5688 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -16,10 +16,9 @@ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #include +#include // for std::reference_wrapper #include -#include #include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" @@ -30,15 +29,15 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_action/types.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "realtime_tools/realtime_server_goal_handle.h" @@ -50,19 +49,8 @@ using namespace std::chrono_literals; // NOLINT -namespace rclcpp_action -{ -template -class ServerGoalHandle; -} // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - namespace joint_trajectory_controller { -class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b5e660be54..792938b96f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,9 +30,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ -#include -#include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c6a5169855..b1cca67547 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -14,12 +14,10 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include #include #include #include -#include -#include + #include #include @@ -33,15 +31,11 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..72820c0295 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -30,7 +29,6 @@ #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -44,9 +42,6 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7b68d882ff..e636fad4b7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,7 +25,6 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "joint_trajectory_controller/trajectory.hpp" namespace { From 2b52f0b54168341b2afe94f0aeef92c5fd41381a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jan 2024 10:05:31 +0100 Subject: [PATCH 423/524] Add few warning flags to error (#961) * add conversion, unused-but-set-variable, format, and return-type warnings to error * Add fixes to the added flags * fix linters --- ackermann_steering_controller/CMakeLists.txt | 3 ++- admittance_controller/CMakeLists.txt | 3 ++- bicycle_steering_controller/CMakeLists.txt | 3 ++- diff_drive_controller/CMakeLists.txt | 2 +- effort_controllers/CMakeLists.txt | 3 ++- force_torque_sensor_broadcaster/CMakeLists.txt | 3 ++- forward_command_controller/CMakeLists.txt | 3 ++- gripper_controllers/CMakeLists.txt | 2 +- imu_sensor_broadcaster/CMakeLists.txt | 3 ++- joint_state_broadcaster/CMakeLists.txt | 3 ++- joint_trajectory_controller/CMakeLists.txt | 2 +- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- pid_controller/CMakeLists.txt | 2 +- pid_controller/src/pid_controller.cpp | 4 ++-- position_controllers/CMakeLists.txt | 3 ++- range_sensor_broadcaster/CMakeLists.txt | 2 +- steering_controllers_library/CMakeLists.txt | 3 ++- tricycle_controller/CMakeLists.txt | 3 ++- tricycle_steering_controller/CMakeLists.txt | 3 ++- velocity_controllers/CMakeLists.txt | 3 ++- 20 files changed, 34 insertions(+), 21 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 6ad0e9485f..66f09c5f09 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a8a8832fce..b2c10e0ba5 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 5d662c1fec..7118e9a44d 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..436832c523 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index ad97690655..eae8642fb6 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 2af5500e21..38c984192e 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 15ffe09750..4885c69c8a 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 8edaaf6386..676062f7a3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 3b09d9e72e..6782012c65 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cadc96b4e3..5c383897cc 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 88bb7891a3..5cddb1bf4d 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 792938b96f..2157fdf2a6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %ld:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 81cbe6f006..15e903222a 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 373e941d96..05fee986dd 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -171,10 +171,10 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void + [&](const std::shared_ptr state_msg) -> void { // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(msg); + measured_state_.writeFromNonRT(state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 54a4f94656..18f3cb313a 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 00da395db5..d7002d1b17 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 63de42cf69..88f24d304c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 291f08ad9f..4f6d064dc8 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 4c56d68185..02c9453ace 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 3642ae1cb9..a39cd162fd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 7d0e1d1a38d902146bd3771a82f39ebaed51cb16 Mon Sep 17 00:00:00 2001 From: maurice Date: Sun, 7 Jan 2024 09:03:14 +0100 Subject: [PATCH 424/524] Update deprecated topic name (#964) --- joint_trajectory_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6fe2146802..af495ad14d 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -168,7 +168,7 @@ Subscriber [#f1]_ The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held. -Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. +Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/controller_state`` topic it is much more cumbersome to realize than with the action interface. Publishers From 30eb4b07d630f4b4b5be6080a6ae9469bdab2f18 Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Sun, 7 Jan 2024 01:04:35 -0700 Subject: [PATCH 425/524] Remove robot description param from admittance YAML (#963) --- .../src/admittance_controller_parameters.yaml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index ee1efa67ab..315d0e70d2 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -147,11 +147,6 @@ admittance_controller: } # general settings - robot_description: { - type: string, - description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", - read_only: true - } enable_parameter_update_without_reactivation: { type: bool, default_value: true, From d3c09c2b25a34c63f2f4a3e2426d7f394dac51c0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 8 Jan 2024 10:21:56 +0000 Subject: [PATCH 426/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 9 +++++++++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 7 +++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 21 files changed, 108 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 975ecbf5a6..5496639fdb 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 86ba49d92a..8c824a835a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove robot description param from admittance YAML (`#963 `_) +* Add few warning flags to error (`#961 `_) +* Contributors: Abishalini Sivaraman, Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index d7259fbc75..3bb6a9312c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index eb40ec1db1..cb11b468cc 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6cd3b74a95..3e40d34d4c 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a808f82c70..32702c02e5 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 45e5a8081c..a1c9495f83 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ff2046a41b..4382714914 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e8be87d1f3..d816ae694a 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 4e92131965..42c06fe201 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1b3559af58..c879d40a83 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update deprecated topic name (`#964 `_) +* Add few warning flags to error (`#961 `_) +* [JTC] Cleanup includes (`#943 `_) +* Add rqt_JTC to docs (`#950 `_) +* [JTC] Add console output for tolerance checks (`#932 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, maurice + 4.2.0 (2023-12-12) ------------------ * Cleanup package.xml und clarify tests of JTC. (`#889 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 5924bc6d94..11c341313c 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ * 🚀 Add PID controller 🎉 (`#434 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 2e1e38fa43..7b5c0683d6 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index c92b4867b5..aff21d74d3 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5839d43b68..3cda43afb5 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.2.0 (2023-12-12) ------------------ * 🚀 Add PID controller 🎉 (`#434 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d7c37b2fee..770267b060 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.2.0 (2023-12-12) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 67d9eb85df..c3252754bd 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add rqt_JTC to docs (`#950 `_) +* Contributors: Christoph Fröhlich + 4.2.0 (2023-12-12) ------------------ * Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index fa8cd4ee2f..c6e0fd1864 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Fix ackermann steering odometry (`#921 `_) +* Changing default int values to double in steering controller's yaml file (`#927 `_) +* Contributors: Franz Rammerstorfer, Reza Kermani, Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index d7424ca31a..74ca0d6d84 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3d04f2ba8d..59f279a080 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 98d6523639..a9496caa75 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error (`#961 `_) +* Contributors: Sai Kishor Kothakota + 4.2.0 (2023-12-12) ------------------ From bdb7ad5bdcdfec0cd0b17bb05aaf3cac8229e7b5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 8 Jan 2024 10:22:27 +0000 Subject: [PATCH 427/524] 4.3.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 5496639fdb..6d78ee3116 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 20bfca8003..e8f07df68f 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.2.0 + 4.3.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 8c824a835a..2de3ab97a7 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Remove robot description param from admittance YAML (`#963 `_) * Add few warning flags to error (`#961 `_) * Contributors: Abishalini Sivaraman, Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 478b10bb55..0f28ce3b50 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.2.0 + 4.3.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 3bb6a9312c..44f32ac497 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 2573605890..45e3a54bc4 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.2.0 + 4.3.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 cb11b468cc..e247d39246 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 5cacf41dad..af59483e24 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.2.0 + 4.3.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 3e40d34d4c..1a3db666b3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e42456c828..5be3c321e6 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.2.0 + 4.3.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 32702c02e5..943f493a83 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 154e597c5f..1d9306c69b 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.2.0 + 4.3.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 a1c9495f83..79f06844d1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 324ec1c5be..d410509a15 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.2.0 + 4.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4382714914..fb07533cc8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index ce981141a5..5e3cc15828 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.2.0 + 4.3.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d816ae694a..08ddd07435 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 48a26ead3f..56abba21b6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.2.0 + 4.3.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 42c06fe201..d37f73736d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 300f8b2238..6dda848d42 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.2.0 + 4.3.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 c879d40a83..81f33ebe66 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Update deprecated topic name (`#964 `_) * Add few warning flags to error (`#961 `_) * [JTC] Cleanup includes (`#943 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 1d0929b8b5..91b7a12c2d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.2.0 + 4.3.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 11c341313c..2fe4dff9df 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 7caa3f7856..2156dc3418 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.2.0 + 4.3.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7b5c0683d6..4fd9bf4f89 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8ed752c536..77ae11fa7b 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.2.0 + 4.3.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 aff21d74d3..b52bcc5a4c 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 08e8fd9506..8dca5951a2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.2.0 + 4.3.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 3cda43afb5..ac14d09a08 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ 4.2.0 (2023-12-12) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 793aea9140..4635d8f69a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.2.0 + 4.3.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 770267b060..df1ee3457e 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ 4.2.0 (2023-12-12) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 84ebaea6fa..9bd1172d0b 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.2.0 + 4.3.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 b9aa443081..25ea77ddc6 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.2.0", + version="4.3.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 c3252754bd..5eab8a8a93 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add rqt_JTC to docs (`#950 `_) * Contributors: Christoph Fröhlich diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4e7c622798..195f5fb3a9 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.2.0 + 4.3.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 1e49983693..4b9a16dd3b 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.2.0", + version="4.3.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 c6e0fd1864..668fb65a73 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Fix ackermann steering odometry (`#921 `_) * Changing default int values to double in steering controller's yaml file (`#927 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 85398f1d90..6e22de993e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.2.0 + 4.3.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 74ca0d6d84..bf386ddb4f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4f131da79c..240badc7c5 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.2.0 + 4.3.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 59f279a080..8e2187d977 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f6eb3fff49..0ff0604886 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.2.0 + 4.3.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 a9496caa75..b1e807481d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.3.0 (2024-01-08) +------------------ * Add few warning flags to error (`#961 `_) * Contributors: Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 3d901585db..b57524bf21 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.2.0 + 4.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 6e2736b0ed521e1b236dcf8ab2ede4ef565e97d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 11 Jan 2024 14:15:01 +0100 Subject: [PATCH 428/524] [JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (#967) --- .../src/joint_trajectory_controller_parameters.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 4981489d36..a6358d1015 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -3,7 +3,6 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of joints used by the controller", - read_only: true, validation: { unique<>: null, } @@ -21,7 +20,6 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of command interfaces to claim", - read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -32,7 +30,6 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of state interfaces to claim", - read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], From fdd6142c5ff9eeea2788ffa5529419a834c960b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 11 Jan 2024 15:01:12 +0100 Subject: [PATCH 429/524] [JTC] Cancel goal in on_deactivate (#962) --- .../src/joint_trajectory_controller.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b1cca67547..4a2a801e51 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -975,6 +975,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + rt_has_pending_goal_.writeFromNonRT(false); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled during deactivate transition."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) @@ -1445,7 +1456,6 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - add_new_trajectory_msg(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); From 1b75446015f691b8c024aad297849c39a6245f4a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 11 Jan 2024 15:21:09 +0000 Subject: [PATCH 430/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 66 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 6d78ee3116..7c508abca5 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 2de3ab97a7..2af98eecd2 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Remove robot description param from admittance YAML (`#963 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 44f32ac497..e5ec7478a7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e247d39246..f4c725052e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 1a3db666b3..c054d6c880 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 943f493a83..2db9d57256 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 79f06844d1..4d325e02eb 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fb07533cc8..8837059171 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 08ddd07435..b6c744a204 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index d37f73736d..1a8fc07a84 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 81f33ebe66..761470b7d3 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cancel goal in on_deactivate (`#962 `_) +* Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García + 4.3.0 (2024-01-08) ------------------ * Update deprecated topic name (`#964 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 2fe4dff9df..91f8e9c343 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 4fd9bf4f89..c6bf03fd3a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b52bcc5a4c..e91f8b65f7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ac14d09a08..b69c108fc6 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index df1ee3457e..56451c00f0 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5eab8a8a93..5ff06b3bff 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add rqt_JTC to docs (`#950 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 668fb65a73..de036d106a 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bf386ddb4f..43f672c3c6 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 8e2187d977..a4dc4c1445 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b1e807481d..c3d0ffe8d3 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-08) ------------------ * Add few warning flags to error (`#961 `_) From 6623cdd2b1078cf6b99ea6827e4c6b68dd39a903 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 11 Jan 2024 15:21:41 +0000 Subject: [PATCH 431/524] 4.4.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7c508abca5..a314802327 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index e8f07df68f..6c318219b2 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.3.0 + 4.4.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 2af98eecd2..826a7aa6be 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 0f28ce3b50..fd7cf32401 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.3.0 + 4.4.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 e5ec7478a7..a1f79c922b 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 45e3a54bc4..2082124de6 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.3.0 + 4.4.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 f4c725052e..316cd9e52d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index af59483e24..9f79dabea6 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.3.0 + 4.4.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 c054d6c880..54cdc18d69 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 5be3c321e6..ee5504c672 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.3.0 + 4.4.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 2db9d57256..58191c2f42 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1d9306c69b..be9fc21d16 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.3.0 + 4.4.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 4d325e02eb..e6f5e3d52f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index d410509a15..c0827e9e20 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.3.0 + 4.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8837059171..677d52f784 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 5e3cc15828..9488804f1d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.3.0 + 4.4.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b6c744a204..821ef8b385 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 56abba21b6..184f9f7dd7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.3.0 + 4.4.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 1a8fc07a84..0650f4e762 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 6dda848d42..70d0f7daca 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.3.0 + 4.4.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 761470b7d3..5eb851589c 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ * Cancel goal in on_deactivate (`#962 `_) * Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_) * Contributors: Christoph Fröhlich, Noel Jiménez García diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 91b7a12c2d..bc6e43a1d5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.3.0 + 4.4.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 91f8e9c343..b492f15efa 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 2156dc3418..dc7f9c13be 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.3.0 + 4.4.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c6bf03fd3a..7c3392dae9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 77ae11fa7b..b714afc89f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.3.0 + 4.4.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 e91f8b65f7..77a365009a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 8dca5951a2..d8213112da 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.3.0 + 4.4.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 b69c108fc6..bdb3c388dd 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4635d8f69a..265adde781 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.3.0 + 4.4.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 56451c00f0..eab2bcfcb3 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 9bd1172d0b..f34838f5ff 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.3.0 + 4.4.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 25ea77ddc6..d9c72db506 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.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 5ff06b3bff..0558e15d49 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 195f5fb3a9..29db146abc 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.3.0 + 4.4.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 4b9a16dd3b..02f32191d5 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.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 de036d106a..ec986caddc 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 6e22de993e..90cb82ac8e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.3.0 + 4.4.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 43f672c3c6..a0dccedfe5 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 240badc7c5..cebd332a60 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.3.0 + 4.4.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 a4dc4c1445..3be8c9ec03 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 0ff0604886..ad1c07d396 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.3.0 + 4.4.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 c3d0ffe8d3..8cc9c869ef 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-11) +------------------ 4.3.0 (2024-01-08) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index b57524bf21..5319f55a97 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.3.0 + 4.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 3809db7351fc06d1623e6d08400f8844d57662b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jan 2024 11:13:39 +0100 Subject: [PATCH 432/524] Update mergify.yml (#974) --- .github/mergify.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 0a6e425a30..e2e225980f 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,13 +32,13 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? - name: development targets master branch conditions: - base!=master - author!=bmagyar - - author!=dstogl + - author!=destogl - author!=christophfroehlich - author!=mergify[bot] - author!=dependabot[bot] From d5f1eabb92bed62543f2223008963170b7542f19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jan 2024 18:29:23 +0100 Subject: [PATCH 433/524] [PID] Remove joint_jog include (#975) --- pid_controller/include/pid_controller/pid_controller.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index a34dc9f87f..f7b8cc4491 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -35,7 +35,6 @@ #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/joint_controller_state.hpp" -#include "control_msgs/msg/joint_jog.hpp" #include "control_msgs/msg/pid_state.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" From 2f7a7552bdd485b540b292d7e541e17519915166 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 17 Jan 2024 07:41:18 +0000 Subject: [PATCH 434/524] Bump actions/upload-artifact from 4.0.0 to 4.1.0 (#976) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index a1d159541d..4b52ea32a3 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.1.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 08e6eafd82..0720291062 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.1.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index f785652989..c65ee8339b 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.1.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index fcc1a297fd..9157fa6edb 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.1.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 833ed7fbf67f0443ca28a1d24e035a64c99f2640 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 23 Jan 2024 00:08:43 +0100 Subject: [PATCH 435/524] [JTC] Convert lambda to class functions (#945) * Convert lambdas to member functions * Make member function const * Add a test for compute_error function * Make reference_wrapper argument const * Iterate over error fields --- .../joint_trajectory_controller.hpp | 35 +++- .../tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 70 +++---- .../test/test_trajectory_controller.cpp | 174 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 24 +++ 5 files changed, 261 insertions(+), 44 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 0ed2ce5688..b9097b1ce3 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 @@ -209,6 +209,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void goal_accepted_callback( std::shared_ptr> goal_handle); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + + /** + * Computes the error for a specific joint in the trajectory. + * + * @param[out] error The computed error for the joint. + * @param[in] index The index of the joint in the trajectory. + * @param[in] current The current state of the joints. + * @param[in] desired The desired state of the joints. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const; // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -217,7 +231,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // sorts the joints of the incoming message to our local order JOINT_TRAJECTORY_CONTROLLER_PUBLIC void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); + std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -251,7 +265,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_active_trajectory() const; - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, @@ -283,6 +296,24 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + + /** + * @brief Assigns the values from a trajectory point interface to a joint interface. + * + * @tparam T The type of the joint interface. + * @param[out] joint_interface The reference_wrapper to assign the values to + * @param[in] trajectory_point_interface Containing the values to assign. + * @todo: Use auto in parameter declaration with c++20 + */ + template + void assign_interface_from_point( + const T & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + } }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 2157fdf2a6..c46b1c297f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %ld:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a2a801e51..a8c7562b2f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -122,35 +122,6 @@ controller_interface::return_type JointTrajectoryController::update( } } - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, size_t index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) - { - // error defined as the difference between current and desired - if (joints_angle_wraparound_[index]) - { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piupdate(*new_external_msg); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; - // current state update state_current_.time_from_start.set__sec(0); read_state_from_state_interfaces(state_current_); @@ -1191,6 +1151,34 @@ void JointTrajectoryController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } +void JointTrajectoryController::compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const +{ + // error defined as the difference between current and desired + if (joints_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi trajectory_msg) const { @@ -1252,7 +1240,7 @@ void JointTrajectoryController::fill_partial_goal( } void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) + std::shared_ptr trajectory_msg) const { // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 72820c0295..dce04bf43a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -461,6 +461,180 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=true + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // angle wraparound of position error + desired.positions[0] += 3.0 * M_PI_2; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + if (i == 0) + { + EXPECT_NEAR( + error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS); + } + else + { + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + } + + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=false + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // no normalization of position error + desired.positions[0] += 3.0 * M_PI_4; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are angle_wraparound if not configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index e636fad4b7..f98fd3e286 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -107,6 +107,13 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } + void testable_compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + compute_error_for_joint(error, index, current, desired); + } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; @@ -154,6 +161,23 @@ class TestableJointTrajectoryController trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + /** + * a copy of the private member function + */ + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + { + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From d69676709afe8f82bdeb44f665dad66eaca3d5da Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 09:43:12 +0000 Subject: [PATCH 436/524] Bump actions/upload-artifact from 4.1.0 to 4.2.0 (#984) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 4b52ea32a3..a195702d8c 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.1.0 + - uses: actions/upload-artifact@v4.2.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 0720291062..7febd4417c 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.1.0 + - uses: actions/upload-artifact@v4.2.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index c65ee8339b..c00f15e4a6 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.1.0 + - uses: actions/upload-artifact@v4.2.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 9157fa6edb..0f20551214 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.1.0 + - uses: actions/upload-artifact@v4.2.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 3ab375942e2c0086d086d1d83a470449ae25351f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 23 Jan 2024 10:50:42 +0100 Subject: [PATCH 437/524] Update mergify.yml (#982) --- .github/mergify.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index e2e225980f..39ee6b6bc0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -46,5 +46,5 @@ pull_request_rules: comment: message: | @{{author}}, all pull requests must be targeted towards the `master` development branch. - Once merged into `master`, it is possible to backport to @{{base}}, but it must be in `master` + Once merged into `master`, it is possible to backport to `{{base}}`, but it must be in `master` to have these changes reflected into new distributions. From 0bcb77bf083894d8208d6bfe7ae560d7dbf90aab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 25 Jan 2024 18:19:27 +0100 Subject: [PATCH 438/524] [CI] debian + RHEL updates (#985) --- .github/workflows/humble-debian-build.yml | 30 +++++++++++++++++++ .../workflows/humble-rhel-binary-build.yml | 20 ++++++------- .github/workflows/iron-debian-build.yml | 30 +++++++++++++++++++ .github/workflows/iron-rhel-binary-build.yml | 18 +++++++---- .github/workflows/rolling-debian-build.yml | 30 +++++++++++++++++++ .../workflows/rolling-rhel-binary-build.yml | 18 +++++++---- 6 files changed, 123 insertions(+), 23 deletions(-) create mode 100644 .github/workflows/humble-debian-build.yml create mode 100644 .github/workflows/iron-debian-build.yml create mode 100644 .github/workflows/rolling-debian-build.yml diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml new file mode 100644 index 0000000000..e8deb2caa5 --- /dev/null +++ b/.github/workflows/humble-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Humble Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_debian: + name: Humble debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: ghcr.io/ros-controls/ros:humble-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 9da6059892..cd9b85b2e1 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -1,19 +1,13 @@ -name: Humble RHEL Binary Build +name: RHEL Humble 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 @@ -25,9 +19,13 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml new file mode 100644 index 0000000000..09dbd051b2 --- /dev/null +++ b/.github/workflows/iron-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Iron Build +on: + workflow_dispatch: + pull_request: + branches: + - iron + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + iron_debian: + name: Iron debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: iron + container: ghcr.io/ros-controls/ros:iron-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 5664d61768..0eb28b9673 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Iron RHEL Binary Build +name: RHEL Iron Binary Build on: workflow_dispatch: - push: + pull_request: branches: - iron schedule: @@ -20,9 +20,15 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + # source also underlay workspace with generate_parameter_library on rhel9 + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + source /opt/ros2_ws/install/setup.bash + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml new file mode 100644 index 0000000000..b6d0a4193a --- /dev/null +++ b/.github/workflows/rolling-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Rolling Build +on: + workflow_dispatch: + pull_request: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + rolling_debian: + name: Rolling debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: ghcr.io/ros-controls/ros:rolling-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 04dc58775f..dece43b673 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Rolling RHEL Binary Build +name: RHEL Rolling Binary Build on: workflow_dispatch: - push: + pull_request: branches: - master schedule: @@ -20,9 +20,15 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + # source also underlay workspace with generate_parameter_library on rhel9 + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + source /opt/ros2_ws/install/setup.bash + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose From 9f7e9e916ce8ae1403f0f0f5d108c80ea0242a9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 25 Jan 2024 19:02:41 +0100 Subject: [PATCH 439/524] Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (#967)" (#978) This reverts commit 6e2736b0ed521e1b236dcf8ab2ede4ef565e97d4. --- .../src/joint_trajectory_controller_parameters.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index a6358d1015..4981489d36 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -3,6 +3,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -20,6 +21,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of command interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -30,6 +32,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of state interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], From 7fee9401d68302e27ffa78efe6d1dc82237c39e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jan 2024 18:55:33 +0100 Subject: [PATCH 440/524] Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (#698)" (#988) This reverts commit 32aaef7552638826aba0b3f3a72b1c1453739afa. --- .../src/force_torque_sensor_broadcaster.cpp | 16 +++-- .../test_force_torque_sensor_broadcaster.cpp | 67 +++++++++---------- 2 files changed, 39 insertions(+), 44 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e9a173380b..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -29,12 +29,6 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() } controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { try { @@ -43,10 +37,18 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } catch (const std::exception & e) { - fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + const bool no_interface_names_defined = params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && 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 ce6a04ec1f..f06252d83a 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 @@ -113,12 +113,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -129,7 +128,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_node()->declare_parameter("sensor_name", ""); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -140,8 +139,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", ""); - fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", ""); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -152,10 +151,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -172,12 +171,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -188,8 +186,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -216,8 +214,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -232,10 +230,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -249,8 +246,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -272,10 +269,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -297,16 +293,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.y", "fts_sensor/force.y"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.z", "fts_sensor/force.z"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.x", "fts_sensor/torque.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.y", "fts_sensor/torque.y"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); From 8d732f1a3bc2879f89162e7d0ddae275180451c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Jan 2024 18:58:52 +0100 Subject: [PATCH 441/524] [tricycle_controller] Use generate_parameter_library (#957) --- tricycle_controller/CMakeLists.txt | 13 +- tricycle_controller/doc/userdoc.rst | 10 +- .../tricycle_controller.hpp | 27 +--- tricycle_controller/package.xml | 1 + .../src/tricycle_controller.cpp | 151 +++++------------- .../src/tricycle_controller_parameter.yaml | 149 +++++++++++++++++ .../test/test_load_tricycle_controller.cpp | 9 +- .../test/test_tricycle_controller.cpp | 115 ++++++------- 8 files changed, 275 insertions(+), 200 deletions(-) create mode 100644 tricycle_controller/src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 4f6d064dc8..cdc69f182a 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces controller_interface geometry_msgs + generate_parameter_library hardware_interface nav_msgs pluginlib @@ -29,6 +30,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(tricycle_controller_parameters + src/tricycle_controller_parameter.yaml +) + add_library(tricycle_controller SHARED src/tricycle_controller.cpp src/odometry.cpp @@ -40,6 +45,7 @@ target_include_directories(tricycle_controller PUBLIC $ $ ) +target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -50,8 +56,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_tricycle_controller - test/test_tricycle_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + test/test_tricycle_controller.cpp) target_link_libraries(test_tricycle_controller tricycle_controller ) @@ -66,8 +71,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_tricycle_controller + add_rostest_with_parameters_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml ) target_link_libraries(test_load_tricycle_controller tricycle_controller @@ -85,6 +91,7 @@ install( install( TARGETS tricycle_controller + tricycle_controller_parameters EXPORT export_tricycle_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index d153aeacba..991627eca2 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,7 +5,8 @@ tricycle_controller ===================== -Controller for mobile robots with tricycle drive. +Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. + Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. @@ -24,3 +25,10 @@ Other features Odometry publishing Velocity, acceleration and jerk limits Automatic stop after command timeout + +Parameters +-------------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cef90d026a..24aaf89de9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -45,6 +45,9 @@ #include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" +// auto-generated by generate_parameter_library +#include "tricycle_controller_parameters.hpp" + namespace tricycle_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -109,31 +112,14 @@ class TricycleController : public controller_interface::ControllerInterface double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); std::tuple twist_to_ackermann(double linear_command, double angular_command); - std::string traction_joint_name_; - std::string steering_joint_name_; + // Parameters from ROS for tricycle_controller + std::shared_ptr param_listener_; + Params params_; // HACK: put into vector to avoid initializing structs because they have no default constructors std::vector traction_joint_; std::vector steering_joint_; - struct WheelParams - { - double wheelbase = 0.0; - double radius = 0.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in separate node - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; - - bool publish_ackermann_command_ = false; std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; @@ -168,7 +154,6 @@ class TricycleController : public controller_interface::ControllerInterface SteeringLimiter limiter_steering_; bool is_halted = false; - bool use_stamped_vel_ = true; void reset_odometry( const std::shared_ptr request_header, diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cebd332a60..cc50f0d58c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -10,6 +10,7 @@ Tony Najjar ament_cmake + generate_parameter_library ackermann_msgs backward_ros diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index d6aeec0af1..07be8c2aae 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -52,41 +52,9 @@ CallbackReturn TricycleController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("traction_joint_name", std::string()); - auto_declare("steering_joint_name", std::string()); - - auto_declare("wheelbase", wheel_params_.wheelbase); - auto_declare("wheel_radius", wheel_params_.radius); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - auto_declare("odom_only_twist", odom_params_.odom_only_twist); - - auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); - auto_declare("publish_ackermann_command", publish_ackermann_command_); - auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("traction.max_velocity", NAN); - auto_declare("traction.min_velocity", NAN); - auto_declare("traction.max_acceleration", NAN); - auto_declare("traction.min_acceleration", NAN); - auto_declare("traction.max_deceleration", NAN); - auto_declare("traction.min_deceleration", NAN); - auto_declare("traction.max_jerk", NAN); - auto_declare("traction.min_jerk", NAN); - - auto_declare("steering.max_position", NAN); - auto_declare("steering.min_position", NAN); - auto_declare("steering.max_velocity", NAN); - auto_declare("steering.min_velocity", NAN); - auto_declare("steering.max_acceleration", NAN); - auto_declare("steering.min_acceleration", NAN); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -101,8 +69,8 @@ InterfaceConfiguration TricycleController::command_interface_configuration() con { InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -110,8 +78,8 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const { InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return state_interfaces_config; } @@ -151,7 +119,7 @@ controller_interface::return_type TricycleController::update( double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); } @@ -172,7 +140,7 @@ controller_interface::return_type TricycleController::update( { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) + if (!params_.odom_only_twist) { odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); @@ -186,7 +154,7 @@ controller_interface::return_type TricycleController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -239,7 +207,7 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel @@ -258,74 +226,40 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { auto logger = get_node()->get_logger(); - // update parameters - traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); - steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); - if (traction_joint_name_.empty()) + // update parameters if they have changed + if (param_listener_->is_old(params_)) { - RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); - return CallbackReturn::ERROR; + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); } - if (steering_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); + odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); - odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; - publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; + params_.publish_ackermann_command = + get_node()->get_parameter("publish_ackermann_command").as_bool(); + params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { limiter_traction_ = TractionLimiter( - get_node()->get_parameter("traction.min_velocity").as_double(), - get_node()->get_parameter("traction.max_velocity").as_double(), - get_node()->get_parameter("traction.min_acceleration").as_double(), - get_node()->get_parameter("traction.max_acceleration").as_double(), - get_node()->get_parameter("traction.min_deceleration").as_double(), - get_node()->get_parameter("traction.max_deceleration").as_double(), - get_node()->get_parameter("traction.min_jerk").as_double(), - get_node()->get_parameter("traction.max_jerk").as_double()); + params_.traction.min_velocity, params_.traction.max_velocity, + params_.traction.min_acceleration, params_.traction.max_acceleration, + params_.traction.min_deceleration, params_.traction.max_deceleration, + params_.traction.min_jerk, params_.traction.max_jerk); } catch (const std::invalid_argument & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); return CallbackReturn::ERROR; } - try { limiter_steering_ = SteeringLimiter( - get_node()->get_parameter("steering.min_position").as_double(), - get_node()->get_parameter("steering.max_position").as_double(), - get_node()->get_parameter("steering.min_velocity").as_double(), - get_node()->get_parameter("steering.max_velocity").as_double(), - get_node()->get_parameter("steering.min_acceleration").as_double(), - get_node()->get_parameter("steering.max_acceleration").as_double()); + params_.steering.min_position, params_.steering.max_position, params_.steering.min_velocity, + params_.steering.max_velocity, params_.steering.min_acceleration, + params_.steering.max_acceleration); } catch (const std::invalid_argument & e) { @@ -347,7 +281,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & previous_commands_.emplace(empty_ackermann_drive); // initialize ackermann command publisher - if (publish_ackermann_command_) + if (params_.publish_ackermann_command) { ackermann_command_publisher_ = get_node()->create_publisher( DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -357,7 +291,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (use_stamped_vel_) + if (params_.use_stamped_vel) { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -409,8 +343,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_publisher_); auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = params_.odom_frame_id; + odometry_message.child_frame_id = params_.base_frame_id; // initialize odom values zeros odometry_message.twist = @@ -421,13 +355,12 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message - if (odom_params_.enable_odom_tf) + if (params_.enable_odom_tf) { odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -438,8 +371,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service @@ -456,8 +389,8 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); // Initialize the joints - const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); - const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + const auto wheel_front_result = get_traction(params_.traction_joint_name, traction_joint_); + const auto steering_result = get_steering(params_.steering_joint_name, steering_joint_); if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) { return CallbackReturn::ERROR; @@ -644,12 +577,12 @@ std::tuple TricycleController::twist_to_ackermann(double Vx, dou if (Vx == 0 && theta_dot != 0) { // is spin action alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + Ws = abs(theta_dot) * params_.wheelbase / params_.wheel_radius; return std::make_tuple(alpha, Ws); } - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); - Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, params_.wheelbase); + Ws = Vx / (params_.wheel_radius * std::cos(alpha)); return std::make_tuple(alpha, Ws); } diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml new file mode 100644 index 0000000000..68fc2202c2 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -0,0 +1,149 @@ +tricycle_controller: + traction_joint_name: { + type: string, + default_value: "", + description: "Name of the traction joint", + validation: { + not_empty<>: [] + } + } + steering_joint_name: { + type: string, + default_value: "", + description: "Name of the steering joint", + validation: { + not_empty<>: [] + } + } + wheelbase: { + type: double, + default_value: 0.0, + description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + enable_odom_tf: { + type: bool, + default_value: false, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + odom_only_twist: { + type: bool, + default_value: false, + description: "for doing the pose integration in separate node.", + } + cmd_vel_timeout: { + type: int, + default_value: 500, # milliseconds + description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_ackermann_command: { + type: bool, + default_value: false, + description: "Publish limited commands.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + validation: { + gt<>: [0] + } + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + traction: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_deceleration: { + type: double, + default_value: .NAN, + } + min_deceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + steering: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_position: { + type: double, + default_value: .NAN, + } + min_position: { + type: double, + default_value: .NAN, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 9298fae574..245523c844 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -26,8 +26,6 @@ TEST(TestLoadTricycleController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -39,6 +37,13 @@ TEST(TestLoadTricycleController, load_controller) ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index d8beedae42..018727e260 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -41,6 +41,12 @@ using lifecycle_msgs::msg::State; using testing::SizeIs; using testing::UnorderedElementsAre; +namespace +{ +const char traction_joint_name[] = "traction_joint"; +const char steering_joint_name[] = "steering_joint"; +} // namespace + class TestableTricycleController : public tricycle_controller::TricycleController { public: @@ -146,11 +152,28 @@ class TestTricycleController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::string traction_joint_name_init = traction_joint_name, + const std::string steering_joint_name_init = steering_joint_name, + const std::vector & parameters = {}) + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); + parameter_overrides.push_back( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, "", node_options); + } + const std::string controller_name = "test_tricycle_controller"; std::unique_ptr controller_; - const std::string traction_joint_name = "traction_joint"; - const std::string steering_joint_name = "steering_joint"; double position_ = 0.1; double velocity_ = 0.2; @@ -172,42 +195,24 @@ class TestTricycleController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestTricycleController, configure_fails_without_parameters) +TEST_F(TestTricycleController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +TEST_F(TestTricycleController, init_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ( + InitController(traction_joint_name, std::string()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + InitController(std::string(), steering_joint_name), controller_interface::return_type::ERROR); } TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -215,26 +220,22 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - cmd_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + cmd_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - state_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + state_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -242,15 +243,9 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResources(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -258,15 +253,11 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 1.2), rclcpp::Parameter("wheel_radius", 0.12)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -307,15 +298,11 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); From ab6835144d9d60a38e778484039349ff44844f32 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:15 +0100 Subject: [PATCH 442/524] Bump ros-tooling/action-ros-ci from 0.3.5 to 0.3.6 (#994) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.3.5 to 0.3.6. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.3.5...0.3.6) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index a195702d8c..cf98b2194e 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 7febd4417c..41e40a5cb5 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index c00f15e4a6..7d12c7f2ad 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 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 0f20551214..76c3a70781 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.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From d375e4348347139b6dfce86fcda0a75ce24d0b7b Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:25 +0100 Subject: [PATCH 443/524] Bump codecov/codecov-action from 3.1.4 to 3.1.5 (#993) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.4 to 3.1.5. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.4...v3.1.5) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index cf98b2194e..f1a0941f20 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 41e40a5cb5..1d21f3b78b 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 7d12c7f2ad..e6a5b74d06 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 021ccab8c7a4a75392bd8903aafa1e7c9e23bc01 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:34 +0100 Subject: [PATCH 444/524] Bump actions/upload-artifact from 4.2.0 to 4.3.0 (#992) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 4.2.0 to 4.3.0. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v4.2.0...v4.3.0) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index f1a0941f20..c503e090ab 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 1d21f3b78b..62ff5f34fe 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index e6a5b74d06..3f6dff5ab8 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 76c3a70781..2a3aa21325 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 99fadceed82918975e3471d615e897b3facd536e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 30 Jan 2024 17:44:14 +0100 Subject: [PATCH 445/524] [JTC] Invalidate empty trajectory messages (#902) --- .../src/joint_trajectory_controller.cpp | 6 +++ .../test/test_trajectory_controller.cpp | 42 ++++++++++++++++++- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a8c7562b2f..a22fe7f2d1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1363,6 +1363,12 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + if (trajectory.points.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; + } + if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index dce04bf43a..fbd6d7aea5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1366,6 +1366,11 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.joint_names = {"bad_name"}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // No position data traj_msg = good_traj_msg; traj_msg.points[0].positions.clear(); @@ -1402,8 +1407,41 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or -/// velocities are accepted +/** + * @brief Test invalid velocity at trajectory end with parameter set to false + */ +TEST_P( + TrajectoryControllerTestParameterized, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +{ + rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); + + // empty message (no throw!) + ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); From f375c698aa6b241b596a42c9d6e879708be4526f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:35:22 +0100 Subject: [PATCH 446/524] Let sphinx add parameter description with nested structures to documentation (#652) --- admittance_controller/doc/userdoc.rst | 10 +- .../doc/parameters_context.yaml | 9 ++ diff_drive_controller/doc/userdoc.rst | 17 +- .../doc/parameters_context.yaml | 12 ++ .../doc/userdoc.rst | 26 ++- ..._torque_sensor_broadcaster_parameters.yaml | 3 +- gripper_controllers/doc/userdoc.rst | 17 +- imu_sensor_broadcaster/doc/userdoc.rst | 13 +- ...nt_state_broadcaster_parameter_context.yml | 46 ++++++ joint_state_broadcaster/doc/userdoc.rst | 71 ++------- .../joint_state_broadcaster_parameters.yaml | 11 ++ .../doc/parameters.rst | 148 ++---------------- .../doc/parameters_context.yaml | 13 ++ ...oint_trajectory_controller_parameters.yaml | 40 +++-- range_sensor_broadcaster/doc/userdoc.rst | 16 ++ 15 files changed, 204 insertions(+), 248 deletions(-) create mode 100644 diff_drive_controller/doc/parameters_context.yaml create mode 100644 force_torque_sensor_broadcaster/doc/parameters_context.yaml create mode 100644 joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml create mode 100644 joint_trajectory_controller/doc/parameters_context.yaml diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 0e4469cd50..8056a017d7 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -17,10 +17,14 @@ ROS 2 interface of the controller Parameters ^^^^^^^^^^^ -The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The admittance controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +.. generate_parameter_library_details:: ../src/admittance_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test folder `_: + +.. literalinclude:: ../test/test_params.yaml + :language: yaml Topics ^^^^^^^ diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..81e92806f5 --- /dev/null +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -0,0 +1,9 @@ +linear.x: | + Joint limits structure for the linear ``x``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z: | + Joint limits structure for the rotation about ``z``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index d2dd284cf3..70d0d7ca5c 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -64,17 +64,12 @@ Publishers Parameters ,,,,,,,,,,,, -Check `parameter definition file for details `_. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -Note that the documentation on parameters for joint limits can be found in `their header file `_. -Those parameters are: +.. generate_parameter_library_details:: ../src/diff_drive_controller_parameter.yaml + parameters_context.yaml -linear.x [JointLimits structure] - Joint limits structure for the linear X-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +An example parameter file for this controller can be found in `the test directory `_: -angular.z [JointLimits structure] - Joint limits structure for the rotation about Z-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +.. literalinclude:: ../test/config/test_diff_drive_controller.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/doc/parameters_context.yaml b/force_torque_sensor_broadcaster/doc/parameters_context.yaml new file mode 100644 index 0000000000..6991427316 --- /dev/null +++ b/force_torque_sensor_broadcaster/doc/parameters_context.yaml @@ -0,0 +1,12 @@ +interface_names: | + (optional) Defines custom, per axis interface names. + This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. + It is sufficient that only one ``interface_name`` is defined. + This enables the broadcaster to use force sensing cells with less than six measuring axes. + An example definition is: + + .. code-block:: yaml + + interface_names: + force: + x: example_name/example_interface diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index 053723e8f0..df0430e3bb 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -12,25 +12,17 @@ The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see Parameters ^^^^^^^^^^^ -The interfaces can be defined in two ways, using ``sensor_name`` or ``interface_names`` parameter. -Those two parameters can not be defined at the same time +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -frame_id (mandatory) - Frame in which the output message will be published. +The interfaces can be defined in two ways, using the ``sensor_name`` or the ``interface_names`` parameter: +Those two parameters cannot be defined at the same time. -sensor_name (optional) - Defines sensor name used as prefix for its interfaces. - If used standard interface names for a 6D FTS will be used: /force.x, ..., /torque.z. +Full list of parameters: -interface_names.[force|torque].[x|y|z] (optional) - Defines custom, per axis interface names. - This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. - It is sufficient that only one ``interface_name`` is defined. - This enables broadcaster use for force sensing cells with less then six measuring axes. - Example definition is: +.. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml + parameters_context.yaml - .. code-block:: yaml +An example parameter file for this controller can be found in `the test directory `_: - interface_names: - force: - x: example_name/example_interface +.. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 68a85d9d8e..3e75ab6012 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -10,7 +10,8 @@ force_torque_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined. + If used, standard interface names for a 6D FTS will be used: ``/force.x, ..., /torque.z``", } interface_names: force: diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst index 28262e90f9..7f51c8f4ac 100644 --- a/gripper_controllers/doc/userdoc.rst +++ b/gripper_controllers/doc/userdoc.rst @@ -5,10 +5,23 @@ Gripper Action Controller -------------------------------- -Controller for executing a gripper command action for simple single-dof grippers. +Controllers for executing a gripper command action for simple single-dof grippers: + +- ``position_controllers/GripperActionController`` +- ``effort_controllers/GripperActionController`` Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +These controllers use the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/gripper_action_controller_parameters.yaml diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 3b8ae172db..09b51a7ecb 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -11,6 +11,17 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +List of parameters +========================= .. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/imu_sensor_broadcaster_params.yaml + :language: yaml diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml new file mode 100644 index 0000000000..c96f8301ae --- /dev/null +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -0,0 +1,46 @@ +map_interface_to_joint_state: | + Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. + Usecases: + + #. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. + Typically one would map both values in separate interfaces in the framework. + To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. + #. A robot provides multiple measuring techniques for its joint values which results in slightly different values. + Typically one would use separate interface for providing those values in the framework. + Using multiple joint_state_broadcaster instances we could publish and show both in RViz. + + Format (each line is optional): + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: + \t\tvelocity: + \t\teffort: + + + Examples: + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: kf_estimated_position + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tvelocity: derived_velocity + \t\teffort: derived_effort + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: torque_sensor + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: current_sensor diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c9164ec723..c7bf4fa9a1 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -22,70 +22,19 @@ If none of the requested interface are not defined, the controller returns error Parameters ---------- +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -use_local_topics - Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. +List of parameters +,,,,,,,,,,,,,,,,,, -joints - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``interfaces`` parameter. - Joint state broadcaster asks for access to all defined interfaces on all defined joints. +.. generate_parameter_library_details:: + ../src/joint_state_broadcaster_parameters.yaml + joint_state_broadcaster_parameter_context.yml -interfaces - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``joints`` parameter. +An example parameter file +,,,,,,,,,,,,,,,,,,,,,,,,, - -extra_joints - Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. - - -map_interface_to_joint_state - Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. - Usecases: - - 1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. - Typically one would map both values in separate interfaces in the framework. - To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. - - 1. A robot provides multiple measuring techniques for its joint values which results in slightly different values. - Typically one would use separate interface for providing those values in the framework. - Using multiple joint_state_broadcaster instances we could publish and show both in RViz. - - Format (each line is optional): - - .. code-block:: yaml - - map_interface_to_joint_state: - position: - velocity: - effort: - - - Examples: - - .. code-block:: yaml - - map_interface_to_joint_state: - position: kf_estimated_position - - - .. code-block:: yaml - - map_interface_to_joint_state: - velocity: derived_velocity - effort: derived_effort - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: torque_sensor - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: current_sensor +.. generate_parameter_library_default:: + ../src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index ba0d4f0051..8f0d4da6c5 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -2,18 +2,29 @@ joint_state_broadcaster: use_local_topics: { type: bool, default_value: false, + description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published. + Joint state broadcaster asks for access to all defined interfaces on all defined joints." } extra_joints: { type: string_array, default_value: [], + description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0." } interfaces: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``joints`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published." } map_interface_to_joint_state: position: { diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 8ad2b406d6..dbb50fcbeb 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -5,146 +5,18 @@ Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joints (list(string)) - Joint names to control and listen to. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -command_joints (list(string)) - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. -command_interface (list(string)) - Command interfaces provided by the hardware interface for all joints. +List of parameters +========================= - Values: [position | velocity | acceleration] (multiple allowed) +.. generate_parameter_library_details:: + ../src/joint_trajectory_controller_parameters.yaml + parameters_context.yaml -state_interfaces (list(string)) - State interfaces provided by the hardware for all joints. +An example parameter file +========================= - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. - -action_monitor_rate (double) - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). - - Default: 20.0 - -allow_partial_joints_goal (boolean) - Allow joint goals defining trajectory for only some joints. - - Default: false - -allow_integration_in_goal_trajectories (boolean) - Allow integration in goal trajectories to accept goals without position or velocity specified - - Default: false - -interpolation_method (string) - The type of interpolation to use, if any. Can be "splines" or "none". - - Default: splines - -open_loop_control (boolean) - Use controller in open-loop control mode: - - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - * It deactivates the feedback control, see the ``gains`` structure. - - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - - .. Note:: - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - - Default: false - -start_with_holding (bool) - If true, start with holding position after activation. Otherwise, no command will be sent until - the first trajectory is received. - - Default: true - -allow_nonzero_velocity_at_trajectory_end (boolean) - If false, the last velocity point has to be zero or the goal will be rejected. - - Default: false - -cmd_timeout (double) - Timeout after which the input command is considered stale. - Timeout is counted from the end of the trajectory (the last point). - ``cmd_timeout`` must be greater than ``constraints.goal_time``, - otherwise ignored. - - If zero, timeout is deactivated" - - Default: 0.0 - -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. - -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. - - Default: 0.01 - -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. - If set to zero, the controller will wait a potentially infinite amount of time. - - Default: 0.0 (not checked) - -constraints..trajectory (double) - Maximally allowed deviation from the target trajectory for a given joint. - - Default: 0.0 (tolerance is not enforced) - -constraints..goal (double) - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. - - Default: 0.0 (tolerance is not enforced) - -gains (structure) - Only relevant, if ``open_loop_control`` is not set. - - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. - This structure contains the controller gains for every joint with the control law - - .. math:: - - u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), - the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. - -gains..p (double) - Proportional gain :math:`k_p` for PID - - Default: 0.0 - -gains..i (double) - Integral gain :math:`k_i` for PID - - Default: 0.0 - -gains..d (double) - Derivative gain :math:`k_d` for PID - - Default: 0.0 - -gains..i_clamp (double) - Integral clamp. Symmetrical in both positive and negative direction. - - Default: 0.0 - -gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_{ff}` of velocity - - Default: 0.0 - -gains..angle_wraparound (bool) - For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - 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 +.. generate_parameter_library_default:: + ../src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..2ffe8aed36 --- /dev/null +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -0,0 +1,13 @@ +constraints: + Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. + +gains: | + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 4981489d36..ded5bb7ca3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -2,7 +2,7 @@ joint_trajectory_controller: joints: { type: string_array, default_value: [], - description: "Names of joints used by the controller", + description: "Joint names to control and listen to", read_only: true, validation: { unique<>: null, @@ -11,7 +11,7 @@ joint_trajectory_controller: command_joints: { type: string_array, default_value: [], - description: "Names of the commanding joints used by the controller", + description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.", read_only: true, validation: { unique<>: null, @@ -20,7 +20,7 @@ joint_trajectory_controller: command_interfaces: { type: string_array, default_value: [], - description: "Names of command interfaces to claim", + description: "Command interfaces provided by the hardware interface for all joints", read_only: true, validation: { unique<>: null, @@ -31,7 +31,7 @@ joint_trajectory_controller: state_interfaces: { type: string_array, default_value: [], - description: "Names of state interfaces to claim", + description: "State interfaces provided by the hardware for all joints.", read_only: true, validation: { unique<>: null, @@ -42,12 +42,21 @@ joint_trajectory_controller: allow_partial_joints_goal: { type: bool, default_value: false, - description: "Goals with partial set of joints are allowed", + description: "Allow joint goals defining trajectory for only some joints.", } open_loop_control: { type: bool, default_value: false, - description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + description: "Use controller in open-loop control mode + \n\n + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + * It deactivates the feedback control, see the ``gains`` structure. + \n\n + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + \n\n + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n", read_only: true, } allow_integration_in_goal_trajectories: { @@ -58,7 +67,7 @@ joint_trajectory_controller: action_monitor_rate: { type: double, default_value: 20.0, - description: "Rate status changes will be monitored", + description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)", read_only: true, validation: { gt_eq: [0.1] @@ -83,7 +92,7 @@ joint_trajectory_controller: default_value: 0.0, # seconds description: "Timeout after which the input command is considered stale. Timeout is counted from the end of the trajectory (the last point). - cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. If zero, timeout is deactivated", } gains: @@ -91,17 +100,17 @@ joint_trajectory_controller: p: { type: double, default_value: 0.0, - description: "Proportional gain for PID" + description: "Proportional gain :math:`k_p` for PID" } i: { type: double, default_value: 0.0, - description: "Integral gain for PID" + description: "Integral gain :math:`k_i` for PID" } d: { type: double, default_value: 0.0, - description: "Derivative gain for PID" + description: "Derivative gain :math:`k_d` for PID" } i_clamp: { type: double, @@ -111,13 +120,16 @@ joint_trajectory_controller: ff_velocity_scale: { type: double, default_value: 0.0, - description: "Feed-forward scaling of velocity." + description: "Feed-forward scaling :math:`k_{ff}` of velocity" } angle_wraparound: { type: bool, default_value: false, - description: "For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." + description: 'For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + 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.' } constraints: stopped_velocity_tolerance: { diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst index e35bec67ad..39fb98b3aa 100644 --- a/range_sensor_broadcaster/doc/userdoc.rst +++ b/range_sensor_broadcaster/doc/userdoc.rst @@ -11,5 +11,21 @@ The controller is a wrapper around ``RangeSensor`` semantic component (see ``con Parameters ^^^^^^^^^^^ +The Range Sensor Broadcaster uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/range_sensor_broadcaster_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/range_sensor_broadcaster_params.yaml + :language: yaml From f2f36c0c65607659cb5961f1354a64b3d1a6866c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:40:02 +0100 Subject: [PATCH 447/524] Add tests for `interface_configuration_type` consistently (#899) --- .../test_ackermann_steering_controller.cpp | 28 +++++++------- .../test_ackermann_steering_controller.hpp | 2 +- ...kermann_steering_controller_preceeding.cpp | 28 +++++++------- .../test/test_admittance_controller.cpp | 3 ++ .../test/test_bicycle_steering_controller.cpp | 22 +++++------ .../test/test_bicycle_steering_controller.hpp | 2 +- ...bicycle_steering_controller_preceeding.cpp | 22 +++++------ .../test/test_diff_drive_controller.cpp | 12 +++--- .../test_force_torque_sensor_broadcaster.cpp | 6 +++ .../test/test_forward_command_controller.cpp | 9 +++++ ...i_interface_forward_command_controller.cpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 6 +++ .../test/test_joint_state_broadcaster.cpp | 18 +++++++++ .../test/test_trajectory_controller.cpp | 12 +++--- pid_controller/test/test_pid_controller.cpp | 29 ++++++++------- .../test/test_pid_controller_preceding.cpp | 29 ++++++++------- .../test/test_range_sensor_broadcaster.cpp | 4 ++ .../test_steering_controllers_library.cpp | 28 +++++++------- .../test_steering_controllers_library.hpp | 2 +- .../test_tricycle_steering_controller.cpp | 37 ++++++++++--------- .../test_tricycle_steering_controller.hpp | 2 +- ...ricycle_steering_controller_preceeding.cpp | 34 +++++++++-------- 22 files changed, 198 insertions(+), 139 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 480e90e166..ef5454a16c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -46,41 +46,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a2849d5742..7c279d6323 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -62,7 +62,7 @@ class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 2d951588c5..1a16bed838 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -48,41 +48,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index fe1d3214e0..6b03249df8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -157,12 +157,15 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 06b0c7e846..3dcdc0b1db 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -44,29 +44,29 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 521506762b..6e84342bea 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -60,7 +60,7 @@ class TestableBicycleSteeringController : public bicycle_steering_controller::BicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, update_success); FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 875910ba23..bc3a182753 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -46,31 +46,31 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], + cmd_if_conf.names[CMD_TRACTION_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index eb970d34a3..4ad293298f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -249,12 +249,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_THAT( - controller_->state_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); - ASSERT_THAT( - controller_->command_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) 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 f06252d83a..8b994307fa 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 @@ -162,8 +162,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -196,8 +198,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -205,8 +209,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 236cb14edd..937144b48c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -137,6 +137,7 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -189,6 +190,7 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), @@ -197,8 +199,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -352,6 +356,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -359,8 +364,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -383,8 +390,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( 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 2d3b61e211..f8f073c103 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 @@ -297,6 +297,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command auto command_ptr = std::make_shared(); @@ -322,6 +323,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 83edc044d3..62179a99ff 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -119,8 +119,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -138,8 +140,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -147,8 +151,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3e45740bbc..c10f51aaa2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -180,8 +180,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -227,8 +229,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -274,8 +278,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -321,8 +327,10 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -360,9 +368,11 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT( state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -381,8 +391,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -455,8 +467,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -504,8 +518,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; @@ -547,8 +563,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index fbd6d7aea5..24988618bb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -120,13 +120,13 @@ TEST_P(TrajectoryControllerTestParameterized, activate) auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - auto cmd_interface_config = traj_controller_->command_interface_configuration(); - ASSERT_EQ( - cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_interface_config = traj_controller_->state_interface_configuration(); - ASSERT_EQ( - state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state = ActivateTrajectoryController(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 11f703a1a4..a44347f5f1 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -61,34 +61,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_FALSE(controller_->params_.use_external_measured_states); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_interfaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); - for (size_t i = 0; i < command_interfaces.names.size(); ++i) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { for (const auto & dof_name : dof_names_) { - EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -96,10 +98,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); - EXPECT_EQ( - reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index e0d3051226..3e17e69286 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -49,34 +49,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.command_interface, command_interface_); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { for (const auto & dof_name : reference_and_state_dof_names_) { - EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -84,10 +86,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); - EXPECT_EQ( - reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 7b8e6d0e02..64f68a7e7a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -154,8 +154,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), @@ -164,8 +166,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 81075d1082..0217567a26 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -28,41 +28,43 @@ class SteeringControllersLibraryTest }; // checking if all interfaces, command, state and reference are exported as expected -TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 5caf347ac1..83e6054bd4 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -73,7 +73,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); public: diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 82ba924305..c555de53de 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -45,46 +45,47 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c5f6985d4e..e97e2a45bd 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -61,7 +61,7 @@ class TestableTricycleSteeringController : public tricycle_steering_controller::TricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); FRIEND_TEST(TricycleSteeringControllerTest, update_success); FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index dd72332875..6f2913aeb8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -47,46 +47,48 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } From 2705ca8530ab9dae7bc77747541a7c35ceb4d1f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:47:09 +0100 Subject: [PATCH 448/524] [diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958) --- diff_drive_controller/CMakeLists.txt | 6 +- .../diff_drive_controller.hpp | 6 + .../src/diff_drive_controller.cpp | 16 +- .../src/diff_drive_controller_parameter.yaml | 15 +- .../config/test_diff_drive_controller.yaml | 3 +- .../test/test_diff_drive_controller.cpp | 306 +++++++----------- .../test/test_load_diff_drive_controller.cpp | 10 +- 7 files changed, 143 insertions(+), 219 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 436832c523..d67815b5e0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager 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 554bedba59..72b38f7d2d 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 @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* 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. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ea08aef89b..42b6cda8e1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -149,7 +149,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -166,8 +166,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -257,7 +257,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -286,12 +286,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; @@ -320,7 +314,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 0c0285e7c2..9720e068e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", } - wheels_per_side: { - type: int, - default_value: 0, - 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, default_value: 0.0, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4ad293298f..43dae41a9b 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, ns, node_options); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -193,59 +216,31 @@ class TestDiffDriveController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -259,26 +254,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -292,26 +279,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -327,26 +306,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -363,26 +334,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -398,26 +362,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -435,26 +392,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -469,13 +419,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -483,15 +427,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -499,15 +437,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -516,15 +450,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -533,15 +463,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -550,15 +476,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -599,15 +521,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 1eb8939031..983ec6d98f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -21,8 +21,6 @@ TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -33,6 +31,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } From 67dbf7b4128f75f8d907d8ca8d480e3108bf9bca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:48:22 +0100 Subject: [PATCH 449/524] [JTC] Fill action error_strings (#887) --- .../src/joint_trajectory_controller.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a22fe7f2d1..b0e93b6ecd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -294,6 +294,7 @@ controller_interface::return_type JointTrajectoryController::update( { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 @@ -310,9 +311,10 @@ controller_interface::return_type JointTrajectoryController::update( { if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -325,17 +327,19 @@ controller_interface::return_type JointTrajectoryController::update( } else if (!within_goal_time) { + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); From 08ea6cf3885bd286cfd17fb1b32b40a763afae61 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 20:54:51 +0000 Subject: [PATCH 450/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 10 ++++++++++ pid_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 107 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a314802327..83e135693c 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 826a7aa6be..47f5d2ac30 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a1f79c922b..c44e1a9157 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 316cd9e52d..d3f89a9eea 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 54cdc18d69..debb1f7717 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 58191c2f42..5b0be8fdbe 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.4.0 (2024-01-11) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e6f5e3d52f..6241e56ce1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 677d52f784..5143c40545 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 821ef8b385..4d3b88cbd9 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 0650f4e762..d9dc1645e2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5eb851589c..b6d85a29b5 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Fill action error_strings (`#887 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* [JTC] Invalidate empty trajectory messages (`#902 `_) +* Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_)" (`#978 `_) +* [JTC] Convert lambda to class functions (`#945 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García + 4.4.0 (2024-01-11) ------------------ * Cancel goal in on_deactivate (`#962 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index b492f15efa..c2a5e3a117 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* [PID] Remove joint_jog include (`#975 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7c3392dae9..2268ccc082 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77a365009a..6d280dcd72 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bdb3c388dd..6f3c02f142 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index eab2bcfcb3..17033eae28 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0558e15d49..504366c761 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ec986caddc..3483a6d388 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a0dccedfe5..075b1f516e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [tricycle_controller] Use generate_parameter_library (`#957 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3be8c9ec03..184ffd20d5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8cc9c869ef..25bec1853d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ From fd8b91f74e9d6436b618f7b0daa252db55e90c73 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 20:55:20 +0000 Subject: [PATCH 451/524] 4.5.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 83e135693c..3521258b1a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 6c318219b2..366c5c31cf 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.4.0 + 4.5.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 47f5d2ac30..ba1a25c0ab 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index fd7cf32401..d379438824 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.4.0 + 4.5.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 c44e1a9157..44d8e5051d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 2082124de6..cddfcf3975 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.4.0 + 4.5.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 d3f89a9eea..4a517f8a6d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9f79dabea6..4531a5337d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.4.0 + 4.5.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 debb1f7717..2cb40f6e36 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ee5504c672..ce2700ef01 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.4.0 + 4.5.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 5b0be8fdbe..fc05235ade 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index be9fc21d16..1ebd0c4a79 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.4.0 + 4.5.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 6241e56ce1..d0e32e8793 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c0827e9e20..cae9f877b7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5143c40545..9013897769 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 9488804f1d..348cf6dd93 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.4.0 + 4.5.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d3b88cbd9..61d843c659 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 184f9f7dd7..b6fc59d85c 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.4.0 + 4.5.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 d9dc1645e2..cd80339155 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 70d0f7daca..43dafb66b0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.4.0 + 4.5.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 b6d85a29b5..a41c70cbc7 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * [JTC] Fill action error_strings (`#887 `_) * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index bc6e43a1d5..030d91d3e9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.4.0 + 4.5.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c2a5e3a117..40bfd292f5 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * [PID] Remove joint_jog include (`#975 `_) * Contributors: Christoph Fröhlich diff --git a/pid_controller/package.xml b/pid_controller/package.xml index dc7f9c13be..4aa553f31a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.4.0 + 4.5.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 2268ccc082..e62920bf7f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b714afc89f..e6a98ef7d2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.4.0 + 4.5.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 6d280dcd72..09ce2703a7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index d8213112da..c74acce857 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.4.0 + 4.5.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 6f3c02f142..8c96dc0ac0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 265adde781..d8477b3ccd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.4.0 + 4.5.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 17033eae28..ab8b17ea7a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f34838f5ff..4c99d9c18a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.4.0 + 4.5.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 d9c72db506..4c5afefe0a 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.4.0", + version="4.5.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 504366c761..c143c7c6ad 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 29db146abc..0929c2aaa3 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.4.0 + 4.5.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 02f32191d5..399ca590f9 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.4.0", + version="4.5.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 3483a6d388..8d67b186b6 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 90cb82ac8e..66a968b71f 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.4.0 + 4.5.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 075b1f516e..289ff3e6b0 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * [tricycle_controller] Use generate_parameter_library (`#957 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cc50f0d58c..d53e8473a1 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.4.0 + 4.5.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 184ffd20d5..4f8d2be9f4 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index ad1c07d396..0263f8f9fe 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.4.0 + 4.5.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 25bec1853d..cbf4e9d068 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5319f55a97..94d61b8ae1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2817f2779673e43d1c63ee0d8b35c8ed6f85410d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 1 Feb 2024 11:19:13 +0100 Subject: [PATCH 452/524] Fix tests for using new `get_node_options` API (#840) --- .../test/test_ackermann_steering_controller.hpp | 4 +++- .../test/test_bicycle_steering_controller.hpp | 4 +++- .../test/test_diff_drive_controller.cpp | 3 ++- doc/writing_new_controller.rst | 4 +++- .../test/test_joint_group_effort_controller.cpp | 3 ++- .../test/test_force_torque_sensor_broadcaster.cpp | 4 +++- .../test/test_forward_command_controller.cpp | 3 ++- .../test_multi_interface_forward_command_controller.cpp | 4 +++- gripper_controllers/test/test_gripper_controllers.cpp | 3 ++- .../test/test_imu_sensor_broadcaster.cpp | 3 ++- .../test/test_joint_state_broadcaster.cpp | 3 ++- .../test/test_trajectory_controller_utils.hpp | 9 ++++++++- pid_controller/test/test_pid_controller.hpp | 4 +++- .../test/test_joint_group_position_controller.cpp | 3 ++- .../test/test_range_sensor_broadcaster.cpp | 3 ++- .../test/test_steering_controllers_library.hpp | 4 +++- tricycle_controller/test/test_tricycle_controller.cpp | 3 ++- .../test/test_tricycle_steering_controller.hpp | 4 +++- .../test/test_joint_group_velocity_controller.cpp | 3 ++- 19 files changed, 52 insertions(+), 19 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 7c279d6323..a047186d14 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,9 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 6e84342bea..5e21ff228c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 43dae41a9b..9ab3022a9f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -218,7 +218,8 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 501c231def..1a9ffce714 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + + 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. 4. **Adding definitions into source file (.cpp)** diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index f9d72ab202..200a1beda8 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,8 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options()); 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 8b994307fa..2412361352 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 @@ -55,7 +55,9 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); + const auto result = fts_broadcaster_->init( + "test_force_torque_sensor_broadcaster", "", 0, "", + fts_broadcaster_->define_custom_node_options()); 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 937144b48c..c31c8a964c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -62,7 +62,8 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", "", 0); + const auto result = controller_->init( + "forward_command_controller", "", 0, "", controller_->define_custom_node_options()); 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 f8f073c103..7879d5c1d7 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 @@ -64,7 +64,9 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); + const auto result = controller_->init( + "multi_interface_forward_command_controller", "", 0, "", + controller_->define_custom_node_options()); 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 da9e15840e..9f7e024917 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -62,7 +62,8 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", "", 0); + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 62179a99ff..25a39a8b4d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -55,7 +55,8 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); + const auto result = imu_broadcaster_->init( + "test_imu_sensor_broadcaster", "", 0, "", imu_broadcaster_->define_custom_node_options()); 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 c10f51aaa2..2faa55f467 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,8 @@ 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", "", 0); + const auto result = state_broadcaster_->init( + "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); 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 f98fd3e286..6978d0e452 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -66,6 +66,8 @@ class TestableJointTrajectoryController return ret; } + rclcpp::NodeOptions define_custom_node_options() const override { return node_options_; } + /** * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the @@ -157,6 +159,8 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } @@ -179,6 +183,7 @@ class TestableJointTrajectoryController } rclcpp::WaitSet joint_cmd_sub_wait_set_; + rclcpp::NodeOptions node_options_; }; class TrajectoryControllerTest : public ::testing::Test @@ -233,8 +238,10 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); + traj_controller_->set_node_options(node_options); - return traj_controller_->init(controller_name_, "", 0, "", node_options); + return traj_controller_->init( + controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); } void SetPidParameters( diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index ab32f5cb48..1c356263e7 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -146,7 +146,9 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(dof_names_.size()); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 3b4f00be12..60bff556db 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,8 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_position_controller", "", 0, "", controller_->define_custom_node_options()); 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 64f68a7e7a..010f18c1a6 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -37,7 +37,8 @@ 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, "", 0); + result = range_broadcaster_->init( + broadcaster_name, "", 0, "", range_broadcaster_->define_custom_node_options()); 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 83e6054bd4..1284b72096 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,9 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 018727e260..5280aaf244 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -197,7 +197,8 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e97e2a45bd..6a516691b8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + 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 4cbf1b7342..a99ffaeebf 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,8 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_velocity_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From 737a45b170e79c0f6bd033b37c0def6d00ab4adf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 07:22:08 +0100 Subject: [PATCH 453/524] Use correct ref for scheduled workflows (#1013) --- .github/workflows/humble-debian-build.yml | 1 + .github/workflows/humble-rhel-binary-build.yml | 1 + .github/workflows/iron-debian-build.yml | 1 + .github/workflows/iron-rhel-binary-build.yml | 1 + .github/workflows/rolling-debian-build.yml | 2 ++ .github/workflows/rolling-rhel-binary-build.yml | 2 ++ 6 files changed, 8 insertions(+) diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index e8deb2caa5..426b935fa4 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index cd9b85b2e1..933486ba50 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -19,6 +19,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 09dbd051b2..c47fbe5cd9 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 0eb28b9673..c3bc1e6def 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index b6d0a4193a..9169494b00 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index dece43b673..98c02b72a3 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Install dependencies run: | rosdep update From 8bf379b2726900adec452193a777d900ba152e89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 09:38:34 +0100 Subject: [PATCH 454/524] Add test_depend on `hardware_interface_testing` (#1018) --- ackermann_steering_controller/package.xml | 1 + admittance_controller/package.xml | 1 + bicycle_steering_controller/package.xml | 1 + diff_drive_controller/test/test_load_diff_drive_controller.cpp | 3 +++ effort_controllers/package.xml | 2 ++ .../test/test_load_joint_group_effort_controller.cpp | 2 ++ force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/package.xml | 1 + .../test/test_load_gripper_action_controllers.cpp | 2 ++ imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/package.xml | 1 + pid_controller/package.xml | 1 + position_controllers/package.xml | 2 ++ .../test/test_load_joint_group_position_controller.cpp | 2 ++ range_sensor_broadcaster/package.xml | 2 +- tricycle_controller/package.xml | 1 + tricycle_controller/test/test_load_tricycle_controller.cpp | 2 ++ tricycle_steering_controller/package.xml | 2 +- velocity_controllers/package.xml | 1 + .../test/test_load_joint_group_velocity_controller.cpp | 2 ++ 22 files changed, 31 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 366c5c31cf..fe22ca10b8 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d379438824..e690330aa0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -34,6 +34,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing kinematics_interface_kdl ros2_control_test_assets diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index cddfcf3975..8bb6ac79fa 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 983ec6d98f..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -16,6 +16,9 @@ #include #include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ce2700ef01..279d5fbf43 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 61bb1ddf9a..52f1f9934a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1ebd0c4a79..0791eb5d16 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -23,6 +23,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index cae9f877b7..8950a9a3e9 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 348cf6dd93..a35fce7894 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 130b12e0bb..0ef5f0bcb2 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadGripperActionControllers, load_controller) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b6fc59d85c..5694e1cee7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 43dafb66b0..6dd8b4b61e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 030d91d3e9..8cd2e5becc 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 4aa553f31a..70c7bfa987 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e6a98ef7d2..e67d3d8a46 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index fe61039fdb..bc27b5e629 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupPositionController, load_controller) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index c74acce857..2d865c1d7f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -22,7 +22,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index d53e8473a1..4a8725810b 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -30,6 +30,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 245523c844..bd54459780 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -21,7 +21,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadTricycleController, load_controller) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 0263f8f9fe..16bfd522f7 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -29,7 +29,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 94d61b8ae1..3e28f7736e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -17,6 +17,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 1872b5f746..e426349f96 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) From c786604f10516379d9c5460eb5356920d923ae03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 11:04:49 +0100 Subject: [PATCH 455/524] Add test_depend on `hardware_interface_testing` also for diff_drive (#1021) --- diff_drive_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4531a5337d..e87ab85471 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -26,6 +26,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 5cde4fc9545fe5f14ad0fa59db67f022c476061e Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:04:42 +0100 Subject: [PATCH 456/524] Bump codecov/codecov-action from 3.1.5 to 4.0.1 (#1023) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.5 to 4.0.1. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.5...v4.0.1) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index c503e090ab..931a715a17 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 62ff5f34fe..06eadf320d 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3f6dff5ab8..2fdbf677e7 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 89109a0809fe00e2fbb7fd420bbc4c288ebb3852 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:05:48 +0100 Subject: [PATCH 457/524] Bump actions/upload-artifact from 4.3.0 to 4.3.1 (#1024) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 4.3.0 to 4.3.1. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v4.3.0...v4.3.1) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Dr. Denis --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 931a715a17..357ee3dfa8 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 06eadf320d..7914a1acb0 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 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 2fdbf677e7..b96276ca5a 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 2a3aa21325..3d5bc1cf35 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 1266941cefcf02e2a409303a4e5f975e87e4d358 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 9 Feb 2024 11:51:38 +0100 Subject: [PATCH 458/524] [CI] Improvements and Cleanups (#1028) --- .github/workflows/ci-ros-lint.yml | 70 +++++++------------ .../workflows/humble-binary-build-main.yml | 2 - .../workflows/humble-binary-build-testing.yml | 2 - .github/workflows/humble-debian-build.yml | 25 ++----- .../workflows/humble-rhel-binary-build.yml | 25 ++----- .../humble-semi-binary-build-main.yml | 2 - .../humble-semi-binary-build-testing.yml | 2 - .github/workflows/humble-source-build.yml | 2 - .github/workflows/iron-binary-build-main.yml | 4 -- .../workflows/iron-binary-build-testing.yml | 4 -- .github/workflows/iron-debian-build.yml | 25 ++----- .github/workflows/iron-rhel-binary-build.yml | 27 ++----- .../workflows/iron-semi-binary-build-main.yml | 4 -- .../iron-semi-binary-build-testing.yml | 4 -- .github/workflows/iron-source-build.yml | 2 - .github/workflows/reusable-debian-build.yml | 63 +++++++++++++++++ .../reusable-industrial-ci-with-cache.yml | 4 +- .../workflows/reusable-rhel-binary-build.yml | 69 ++++++++++++++++++ .../workflows/rolling-binary-build-main.yml | 4 -- .../rolling-binary-build-testing.yml | 4 -- .github/workflows/rolling-debian-build.yml | 26 ++----- .../workflows/rolling-rhel-binary-build.yml | 28 ++------ .../rolling-semi-binary-build-main.yml | 4 -- .../rolling-semi-binary-build-testing.yml | 4 -- 24 files changed, 199 insertions(+), 207 deletions(-) create mode 100644 .github/workflows/reusable-debian-build.yml create mode 100644 .github/workflows/reusable-rhel-binary-build.yml diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index f6b9c027c9..df17d11dc4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -2,6 +2,30 @@ name: ROS Lint on: pull_request: +env: + 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 + pid_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 + jobs: ament_lint: name: ament_${{ matrix.linter }} @@ -19,28 +43,7 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} - 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 - pid_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 + package-name: ${{ env.package-name }} ament_lint_100: @@ -58,25 +61,4 @@ jobs: distribution: rolling linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" - 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 - pid_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 + package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 64d78f281a..9c634b372a 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 524cacd685..b662543959 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index 426b935fa4..c236aecf64 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Humble Build +name: Debian Humble Source Build on: workflow_dispatch: pull_request: @@ -12,20 +12,9 @@ on: jobs: humble_debian: name: Humble debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 933486ba50..db503d1993 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -11,22 +11,9 @@ on: 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 - ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 863df79a22..bfe83392ea 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index 6286636e1f..3a66c0b74d 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index ff0fd62e05..a40d53f8e3 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,6 @@ name: Humble Source Build on: workflow_dispatch: - branches: - - humble push: branches: - humble diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml index ef35397855..bb1997bd48 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml index 25a693dc23..37e3524ccd 100644 --- a/.github/workflows/iron-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index c47fbe5cd9..58787a804c 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Iron Build +name: Debian Iron Source Build on: workflow_dispatch: pull_request: @@ -12,20 +12,9 @@ on: jobs: iron_debian: name: Iron debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: iron - container: ghcr.io/ros-controls/ros:iron-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index c3bc1e6def..90dac67a44 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -12,24 +12,9 @@ on: jobs: iron_rhel_binary: name: Iron RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: iron - container: ghcr.io/ros-controls/ros:iron-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - # source also underlay workspace with generate_parameter_library on rhel9 - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - source /opt/ros2_ws/install/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml index 2224a59f0e..ed90a46ea8 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml index c5ff430c89..d06a20443d 100644 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 1e9d865c49..34372a4178 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -1,8 +1,6 @@ name: Iron Source Build on: workflow_dispatch: - branches: - - iron push: branches: - iron diff --git a/.github/workflows/reusable-debian-build.yml b/.github/workflows/reusable-debian-build.yml new file mode 100644 index 0000000000..b406fe6eaa --- /dev/null +++ b/.github/workflows/reusable-debian-build.yml @@ -0,0 +1,63 @@ +name: Reusable Debian Source Build +# Reusable action to simplify dealing with debian source builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + + +jobs: + debian_source: + name: ${{ inputs.ros_distro }} debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: ${{ inputs.ros_distro }} + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-debian + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Build workspace + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index acefeebfac..234ec27677 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -56,10 +56,10 @@ jobs: BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - - name: Checkout ${{ inputs.ref }} when build is not scheduled + - name: Checkout default ref when build is not scheduled if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v4 - - name: Checkout ${{ inputs.ref }} on scheduled build + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v4 with: diff --git a/.github/workflows/reusable-rhel-binary-build.yml b/.github/workflows/reusable-rhel-binary-build.yml new file mode 100644 index 0000000000..be4eabb76b --- /dev/null +++ b/.github/workflows/reusable-rhel-binary-build.yml @@ -0,0 +1,69 @@ +name: Reusable RHEL Binary Build +# Reusable action to simplify dealing with RHEL binary builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + +jobs: + rhel_binary: + name: ${{ inputs.ros_distro }} RHEL binary build + runs-on: ubuntu-latest + env: + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-rhel + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Install dependencies + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + rosdep update + rosdep install -iyr --from-path src || true + - name: Build workspace + # source also underlay workspace with generate_parameter_library on rhel9 + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index 793db5d7e5..729d5e38ba 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml index 9b480d99c3..0596aeec56 100644 --- a/.github/workflows/rolling-binary-build-testing.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 9169494b00..153f4df681 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Rolling Build +name: Debian Rolling Source Build on: workflow_dispatch: pull_request: @@ -12,21 +12,9 @@ on: jobs: rolling_debian: name: Rolling debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - # default behavior is correct on master branch - # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 98c02b72a3..31c133ab69 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -12,25 +12,9 @@ on: jobs: rolling_rhel_binary: name: Rolling RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - # default behavior is correct on master branch - # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - # source also underlay workspace with generate_parameter_library on rhel9 - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - source /opt/ros2_ws/install/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml index 8b395e5163..206ca8bd52 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 630881dc0a..b284c0b7d4 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master From e0491baf97bebc8d0a4f01d39623246c30cc4092 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 10 Feb 2024 16:02:15 +0100 Subject: [PATCH 459/524] [JTC] Angle wraparound for first segment of trajectory (#796) --- .../joint_trajectory_controller.hpp | 3 +- .../trajectory.hpp | 26 ++++-- .../src/joint_trajectory_controller.cpp | 6 +- .../src/trajectory.cpp | 32 ++++++- .../test/test_trajectory.cpp | 56 ++++++++++++ .../test/test_trajectory_controller.cpp | 90 +++++++------------ 6 files changed, 148 insertions(+), 65 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 b9097b1ce3..111837cc17 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 @@ -155,7 +155,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 wrapped around + // Configuration for every joint if it wraps around (ie. is continuous, position error is + // normalized) 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/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 3bd4873a31..b00d79481c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -44,14 +44,19 @@ class Trajectory const trajectory_msgs::msg::JointTrajectoryPoint & current_point, std::shared_ptr joint_trajectory); - /// Set the point before the trajectory message is replaced/appended - /// Example: if we receive a new trajectory message and it's first point is 0.5 seconds - /// from the current one, we call this function to log the current state, then - /// append/replace the current trajectory + /** + * Set the point before the trajectory message is replaced/appended + * Example: if we receive a new trajectory message and it's first point is 0.5 seconds + * from the current one, we call this function to log the current state, then + * append/replace the current trajectory + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC void set_point_before_trajectory_msg( const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point); + const trajectory_msgs::msg::JointTrajectoryPoint & current_point, + const std::vector & joints_angle_wraparound = std::vector()); JOINT_TRAJECTORY_CONTROLLER_PUBLIC void update(std::shared_ptr joint_trajectory); @@ -189,6 +194,17 @@ inline std::vector mapping(const T & t1, const T & t2) return mapping_vector; } +/** + * \param current_position The current position given from the controller, which will be adapted. + * \param next_position Next position from which to compute the wraparound offset, i.e., + * the first trajectory point + * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that + * wrap around (ie. is continuous). + */ +void wraparound_joint( + std::vector & current_position, const std::vector next_position, + const std::vector & joints_angle_wraparound); + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b0e93b6ecd..46544bf875 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -153,11 +153,13 @@ controller_interface::return_type JointTrajectoryController::update( first_sample = true; if (params_.open_loop_control) { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg( + time, last_commanded_state_, joints_angle_wraparound_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + traj_external_point_ptr_->set_point_before_trajectory_msg( + time, state_current_, joints_angle_wraparound_); } } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index fae4c41ff9..0ed7f2ff13 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -16,6 +16,7 @@ #include +#include "angles/angles.h" #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" @@ -44,10 +45,39 @@ Trajectory::Trajectory( void Trajectory::set_point_before_trajectory_msg( const rclcpp::Time & current_time, - const trajectory_msgs::msg::JointTrajectoryPoint & current_point) + const trajectory_msgs::msg::JointTrajectoryPoint & current_point, + const std::vector & joints_angle_wraparound) { time_before_traj_msg_ = current_time; state_before_traj_msg_ = current_point; + + // Compute offsets due to wrapping joints + wraparound_joint( + state_before_traj_msg_.positions, trajectory_msg_->points[0].positions, + joints_angle_wraparound); +} + +void wraparound_joint( + std::vector & current_position, const std::vector next_position, + const std::vector & joints_angle_wraparound) +{ + double dist; + // joints_angle_wraparound is even empty, or has the same size as the number of joints + for (size_t i = 0; i < joints_angle_wraparound.size(); i++) + { + if (joints_angle_wraparound[i]) + { + dist = angles::shortest_angular_distance(current_position[i], next_position[i]); + + // Deal with singularity at M_PI shortest distance + if (std::abs(std::abs(dist) - M_PI) < 1e-9) + { + dist = next_position[i] > current_position[i] ? std::abs(dist) : -std::abs(dist); + } + + current_position[i] = next_position[i] - dist; + } + } } void Trajectory::update(std::shared_ptr joint_trajectory) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b52aa67a04..6e0c53ac77 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -821,3 +821,59 @@ TEST(TestTrajectory, skip_interpolation) } } } + +TEST(TestWrapAroundJoint, no_wraparound) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound(3, false); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0]); + EXPECT_EQ(current_position[1], initial_position[1]); + EXPECT_EQ(current_position[2], initial_position[2]); +} + +TEST(TestWrapAroundJoint, wraparound_single_joint) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound{true, false, false}; + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI); + EXPECT_EQ(current_position[1], initial_position[1]); + EXPECT_EQ(current_position[2], initial_position[2]); +} + +TEST(TestWrapAroundJoint, wraparound_all_joints) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector joints_angle_wraparound(3, true); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI); + EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI); + EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI); +} + +TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(next_position); + std::vector joints_angle_wraparound(3, true); + joint_trajectory_controller::wraparound_joint( + current_position, next_position, joints_angle_wraparound); + EXPECT_EQ(current_position[0], next_position[0]); + EXPECT_EQ(current_position[1], next_position[1]); + EXPECT_EQ(current_position[2], next_position[2]); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 24988618bb..2bfe6f150d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -636,14 +636,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal } /** - * @brief check if position error of revolute joints are angle_wraparound if not configured so + * @brief check if position error of revolute joints are wrapped around if not configured so */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); + bool angle_wraparound = false; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound); + subscribeToState(); size_t n_joints = joint_names_.size(); @@ -653,10 +655,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); @@ -720,35 +720,24 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_effort_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions - EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], - k_p * COMMON_THRESHOLD); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); } executor.cancel(); } /** - * @brief check if position error of revolute joints are angle_wraparound if configured so + * @brief check if position error of revolute joints are wrapped around if configured so */ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { @@ -791,7 +780,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); - // is error.positions[2] angle_wraparound? + // is error.positions[2] wrapped around? EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR( @@ -810,15 +799,15 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + // we expect u = k_p * (s_d-s) for joint0 and joint1 EXPECT_NEAR( k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], k_p * COMMON_THRESHOLD); EXPECT_NEAR( k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * COMMON_THRESHOLD); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); + // is error of positions[2] wrapped around? + EXPECT_GT(0.0, joint_vel_[2]); // direction change because of angle wrap EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], k_p * COMMON_THRESHOLD); @@ -835,30 +824,19 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_effort_command_interface()) { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * COMMON_THRESHOLD); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) for joint0 and joint1 + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] wrapped around? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); } executor.cancel(); From 9ab848a5a0f07a68b1cbf53c407fc162d0896a27 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 11 Feb 2024 21:17:41 +0100 Subject: [PATCH 460/524] Fix usage of M_PI on Windows (#1036) --- joint_trajectory_controller/CMakeLists.txt | 2 ++ steering_controllers_library/CMakeLists.txt | 2 +- tricycle_controller/CMakeLists.txt | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5cddb1bf4d..c92d61b3f3 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -58,6 +58,7 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) + target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) @@ -65,6 +66,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_controller joint_trajectory_controller ) + target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 88f24d304c..106fdcc464 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -51,7 +51,7 @@ ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INC # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL" "_USE_MATH_DEFINES") if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index cdc69f182a..94fba916b1 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -47,6 +47,7 @@ target_include_directories(tricycle_controller PUBLIC ) target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(tricycle_controller PRIVATE _USE_MATH_DEFINES) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) From 1fb6100fb42a06d5e9d9f6efc600117b5a65a8de Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 12 Feb 2024 18:28:36 +0000 Subject: [PATCH 461/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 6 ++++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ pid_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 6 ++++++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 7 +++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 6 ++++++ 21 files changed, 120 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 3521258b1a..59fe46a94b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ba1a25c0ab..099091e0db 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Contributors: Christoph Fröhlich + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 44d8e5051d..196f9f9720 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4a517f8a6d..c1ebd19d75 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` also for diff_drive (`#1021 `_) +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 2cb40f6e36..2177f65ec5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index fc05235ade..bbee58772d 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d0e32e8793..4ed48b8b94 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 9013897769..6b8d40d81c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Let sphinx add parameter description with nested structures to documentation (`#652 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 61d843c659..b4f1948271 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cd80339155..a95c23fa5c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a41c70cbc7..15e115f030 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix usage of M_PI on Windows (`#1036 `_) +* [JTC] Angle wraparound for first segment of trajectory (`#796 `_) +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Silvio Traversaro + 4.5.0 (2024-01-31) ------------------ * [JTC] Fill action error_strings (`#887 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 40bfd292f5..e48ec5ef17 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e62920bf7f..d70b881e3d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 09ce2703a7..526f303744 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8c96dc0ac0..e411c6e3f9 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.5.0 (2024-01-31) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index ab8b17ea7a..b397e2eb5c 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.5.0 (2024-01-31) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c143c7c6ad..c0cd9c7743 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.5.0 (2024-01-31) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8d67b186b6..d51ddca848 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix usage of M_PI on Windows (`#1036 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Sai Kishor Kothakota, Silvio Traversaro + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 289ff3e6b0..a366604985 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix usage of M_PI on Windows (`#1036 `_) +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Silvio Traversaro + 4.5.0 (2024-01-31) ------------------ * [tricycle_controller] Use generate_parameter_library (`#957 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4f8d2be9f4..d08fb7c11c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cbf4e9d068..a22b213dc5 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test_depend on `hardware_interface_testing` (`#1018 `_) +* Fix tests for using new `get_node_options` API (`#840 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.5.0 (2024-01-31) ------------------ From 088b69c91b5ddd1d5dce4734d3486e335548d9ff Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 12 Feb 2024 18:29:24 +0000 Subject: [PATCH 462/524] 4.6.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 59fe46a94b..3b77b7b78f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index fe22ca10b8..512af88534 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.5.0 + 4.6.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 099091e0db..65db88f462 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index e690330aa0..2c50dde2e7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.5.0 + 4.6.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 196f9f9720..16192d9f8a 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 8bb6ac79fa..bc560d9bf7 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.5.0 + 4.6.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 c1ebd19d75..1ec4329fca 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` also for diff_drive (`#1021 `_) * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index e87ab85471..b0e818dd50 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.5.0 + 4.6.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 2177f65ec5..ac47c3c2b3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 279d5fbf43..52165f6ec1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.5.0 + 4.6.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 bbee58772d..c61f166b8e 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 0791eb5d16..178a90d5cf 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.5.0 + 4.6.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 4ed48b8b94..54a428ec3f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8950a9a3e9..9b6c2559f4 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.5.0 + 4.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6b8d40d81c..76ea9418d2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a35fce7894..a6b83b43ec 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.5.0 + 4.6.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b4f1948271..d906fb897f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5694e1cee7..5a4ca12e87 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.5.0 + 4.6.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 a95c23fa5c..1444f5f8ab 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 6dd8b4b61e..0bf3db58dd 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.5.0 + 4.6.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 15e115f030..17b26ad551 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Fix usage of M_PI on Windows (`#1036 `_) * [JTC] Angle wraparound for first segment of trajectory (`#796 `_) * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 8cd2e5becc..5aad4cb86e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.5.0 + 4.6.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index e48ec5ef17..7d5d248576 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 70c7bfa987..9cda85aa03 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.5.0 + 4.6.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d70b881e3d..6c55f59602 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e67d3d8a46..13e80f4d99 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.5.0 + 4.6.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 526f303744..62c87c8717 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 2d865c1d7f..3bbe1d2130 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.5.0 + 4.6.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 e411c6e3f9..fc8ca62020 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ 4.5.0 (2024-01-31) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d8477b3ccd..10ae5427e9 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.5.0 + 4.6.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 b397e2eb5c..41a20f3841 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ 4.5.0 (2024-01-31) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 4c99d9c18a..fb5b3d8bcf 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.5.0 + 4.6.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 4c5afefe0a..9219fea906 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.5.0", + version="4.6.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 c0cd9c7743..cd4ae0809e 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ 4.5.0 (2024-01-31) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 0929c2aaa3..107d50681c 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.5.0 + 4.6.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 399ca590f9..8bcb07ac54 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.5.0", + version="4.6.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 d51ddca848..e8288f1955 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Fix usage of M_PI on Windows (`#1036 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Sai Kishor Kothakota, Silvio Traversaro diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 66a968b71f..8212f58d91 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.5.0 + 4.6.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 a366604985..cf2a599b20 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Fix usage of M_PI on Windows (`#1036 `_) * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4a8725810b..04995732f2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.5.0 + 4.6.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 d08fb7c11c..718df4e014 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 16bfd522f7..bed6e451fa 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.5.0 + 4.6.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 a22b213dc5..e6689b23a8 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.6.0 (2024-02-12) +------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) * Fix tests for using new `get_node_options` API (`#840 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 3e28f7736e..ff403e178f 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.5.0 + 4.6.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From cb6884a64a6d1f5e22afd48bb9fe0d8bafc8fece Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 15 Feb 2024 11:35:15 +0100 Subject: [PATCH 463/524] Use reusable wfs from ros2_control_ci and use matrix strategy (#1040) --- .../workflows/humble-binary-build-testing.yml | 24 ----- ...build-main.yml => humble-binary-build.yml} | 9 +- .github/workflows/humble-debian-build.yml | 2 +- ....yml => humble-rhel-semi-binary-build.yml} | 2 +- .../humble-semi-binary-build-main.yml | 23 ----- ...sting.yml => humble-semi-binary-build.yml} | 9 +- .github/workflows/humble-source-build.yml | 2 +- .../workflows/iron-binary-build-testing.yml | 26 ----- ...y-build-main.yml => iron-binary-build.yml} | 9 +- .github/workflows/iron-debian-build.yml | 2 +- ...ld.yml => iron-rhel-semi-binary-build.yml} | 2 +- .../iron-semi-binary-build-testing.yml | 25 ----- ...ld-main.yml => iron-semi-binary-build.yml} | 9 +- .github/workflows/iron-source-build.yml | 2 +- .github/workflows/reusable-debian-build.yml | 63 ------------ .../reusable-industrial-ci-with-cache.yml | 96 ------------------- .../workflows/reusable-rhel-binary-build.yml | 69 ------------- .../reusable-ros-tooling-source-build.yml | 69 ------------- .../rolling-binary-build-testing.yml | 26 ----- ...uild-main.yml => rolling-binary-build.yml} | 9 +- .github/workflows/rolling-debian-build.yml | 2 +- ...yml => rolling-rhel-semi-binary-build.yml} | 7 +- .../rolling-semi-binary-build-testing.yml | 25 ----- ...main.yml => rolling-semi-binary-build.yml} | 9 +- .github/workflows/rolling-source-build.yml | 4 +- 25 files changed, 47 insertions(+), 478 deletions(-) delete mode 100644 .github/workflows/humble-binary-build-testing.yml rename .github/workflows/{humble-binary-build-main.yml => humble-binary-build.yml} (68%) rename .github/workflows/{humble-rhel-binary-build.yml => humble-rhel-semi-binary-build.yml} (82%) delete mode 100644 .github/workflows/humble-semi-binary-build-main.yml rename .github/workflows/{humble-semi-binary-build-testing.yml => humble-semi-binary-build.yml} (65%) delete mode 100644 .github/workflows/iron-binary-build-testing.yml rename .github/workflows/{iron-binary-build-main.yml => iron-binary-build.yml} (70%) rename .github/workflows/{iron-rhel-binary-build.yml => iron-rhel-semi-binary-build.yml} (82%) delete mode 100644 .github/workflows/iron-semi-binary-build-testing.yml rename .github/workflows/{iron-semi-binary-build-main.yml => iron-semi-binary-build.yml} (67%) delete mode 100644 .github/workflows/reusable-debian-build.yml delete mode 100644 .github/workflows/reusable-industrial-ci-with-cache.yml delete mode 100644 .github/workflows/reusable-rhel-binary-build.yml delete mode 100644 .github/workflows/reusable-ros-tooling-source-build.yml delete mode 100644 .github/workflows/rolling-binary-build-testing.yml rename .github/workflows/{rolling-binary-build-main.yml => rolling-binary-build.yml} (70%) rename .github/workflows/{rolling-rhel-binary-build.yml => rolling-rhel-semi-binary-build.yml} (70%) delete mode 100644 .github/workflows/rolling-semi-binary-build-testing.yml rename .github/workflows/{rolling-semi-binary-build-main.yml => rolling-semi-binary-build.yml} (67%) diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml deleted file mode 100644 index b662543959..0000000000 --- a/.github/workflows/humble-binary-build-testing.yml +++ /dev/null @@ -1,24 +0,0 @@ -name: Humble Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - 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-binary-build-main.yml b/.github/workflows/humble-binary-build.yml similarity index 68% rename from .github/workflows/humble-binary-build-main.yml rename to .github/workflows/humble-binary-build.yml index 9c634b372a..989805733f 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build.yml @@ -1,4 +1,4 @@ -name: Humble Binary Build - main +name: Humble Binary Build # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' @@ -16,9 +16,12 @@ on: jobs: binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: humble - ros_repo: main + ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers-not-released.humble.repos ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index c236aecf64..db0a8456f8 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -12,7 +12,7 @@ on: jobs: humble_debian: name: Humble debian build - uses: ./.github/workflows/reusable-debian-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master with: ros_distro: humble upstream_workspace: ros2_controllers.humble.repos diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml similarity index 82% rename from .github/workflows/humble-rhel-binary-build.yml rename to .github/workflows/humble-rhel-semi-binary-build.yml index db503d1993..ccf64a0246 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -11,7 +11,7 @@ on: jobs: humble_rhel_binary: name: Humble RHEL binary build - uses: ./.github/workflows/reusable-rhel-binary-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master with: ros_distro: humble upstream_workspace: ros2_controllers.humble.repos diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml deleted file mode 100644 index bfe83392ea..0000000000 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ /dev/null @@ -1,23 +0,0 @@ -name: Humble Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - 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.yml similarity index 65% rename from .github/workflows/humble-semi-binary-build-testing.yml rename to .github/workflows/humble-semi-binary-build.yml index 3a66c0b74d..1d9d3bd1fb 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: Humble Semi-Binary Build - testing +name: Humble Semi-Binary Build # description: 'Build & test that compiles the main dependencies from source.' on: @@ -15,9 +15,12 @@ on: jobs: semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: humble - ros_repo: testing + ros_repo: ${{ matrix.ROS_REPO }} 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 index a40d53f8e3..7b4427d6d6 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -10,7 +10,7 @@ on: jobs: source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: humble ref: humble diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml deleted file mode 100644 index 37e3524ccd..0000000000 --- a/.github/workflows/iron-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Iron Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - push: - branches: - - iron - 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: iron - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build.yml similarity index 70% rename from .github/workflows/iron-binary-build-main.yml rename to .github/workflows/iron-binary-build.yml index bb1997bd48..146d5a8c69 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build.yml @@ -1,4 +1,4 @@ -name: Iron Binary Build - main +name: Iron Binary Build # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' @@ -18,9 +18,12 @@ on: jobs: binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: iron - ros_repo: main + ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers-not-released.iron.repos ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 58787a804c..e56e8940ad 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -12,7 +12,7 @@ on: jobs: iron_debian: name: Iron debian build - uses: ./.github/workflows/reusable-debian-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master with: ros_distro: iron upstream_workspace: ros2_controllers.iron.repos diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml similarity index 82% rename from .github/workflows/iron-rhel-binary-build.yml rename to .github/workflows/iron-rhel-semi-binary-build.yml index 90dac67a44..66ad427a98 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-semi-binary-build.yml @@ -12,7 +12,7 @@ on: jobs: iron_rhel_binary: name: Iron RHEL binary build - uses: ./.github/workflows/reusable-rhel-binary-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master with: ros_distro: iron upstream_workspace: ros2_controllers.iron.repos diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml deleted file mode 100644 index d06a20443d..0000000000 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Iron Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - push: - branches: - - iron - 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: iron - ros_repo: testing - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build.yml similarity index 67% rename from .github/workflows/iron-semi-binary-build-main.yml rename to .github/workflows/iron-semi-binary-build.yml index ed90a46ea8..ab508dfc50 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: Iron Semi-Binary Build - main +name: Iron Semi-Binary Build # description: 'Build & test that compiles the main dependencies from source.' on: @@ -17,9 +17,12 @@ on: jobs: semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: iron - ros_repo: main + ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers.iron.repos ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 34372a4178..3609dcfc41 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -10,7 +10,7 @@ on: jobs: source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: iron ref: iron diff --git a/.github/workflows/reusable-debian-build.yml b/.github/workflows/reusable-debian-build.yml deleted file mode 100644 index b406fe6eaa..0000000000 --- a/.github/workflows/reusable-debian-build.yml +++ /dev/null @@ -1,63 +0,0 @@ -name: Reusable Debian Source Build -# Reusable action to simplify dealing with debian source builds -# author: Christoph Froehlich - -on: - workflow_call: - inputs: - ros_distro: - description: 'ROS2 distribution name' - required: true - type: string - ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' - required: false - type: string - upstream_workspace: - description: 'Path to local .repos file.' - default: '' - required: false - type: string - skip_packages: - description: 'Packages to skip from build and test' - default: '' - required: false - type: string - - -jobs: - debian_source: - name: ${{ inputs.ros_distro }} debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: ${{ inputs.ros_distro }} - path: src/ros2_controllers - container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-debian - steps: - - name: Checkout default ref when build is not scheduled - if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v4 - with: - path: ${{ env.path }} - - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build - if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v4 - with: - ref: ${{ inputs.ref_for_scheduled_build }} - path: ${{ env.path }} - - name: Build workspace - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - if [[ -n "${{ inputs.upstream_workspace }}" ]]; then - vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} - fi - colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} - - name: Test workspace - shell: bash - continue-on-error: true - run: | - source /opt/ros2_ws/install/setup.bash - colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} - colcon test-result --verbose diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml deleted file mode 100644 index 234ec27677..0000000000 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ /dev/null @@ -1,96 +0,0 @@ -name: Reusable industrial_ci Workflow with Cache -# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache -# author: Denis Štogl - -on: - workflow_call: - inputs: - ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' - required: false - type: string - - upstream_workspace: - description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' - required: true - type: string - ros_distro: - description: 'ROS_DISTRO variable for industrial_ci' - required: true - type: string - ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' - default: 'main' - required: false - type: string - os_code_name: - description: 'OS_CODE_NAME variable for industrial_ci' - default: '' - required: false - type: string - before_install_upstream_dependencies: - description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' - default: '' - required: false - type: string - - ccache_dir: - description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.ccache' - required: false - type: string - basedir: - description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.work' - required: false - type: string - - -jobs: - reusable_industrial_ci_with_cache: - name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} - runs-on: ubuntu-latest - env: - CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} - BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} - steps: - - name: Checkout default ref when build is not scheduled - if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v4 - - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build - if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v4 - with: - ref: ${{ inputs.ref_for_scheduled_build }} - - name: cache target_ws - if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v3.0.11 - with: - path: ${{ env.BASEDIR }}/target_ws - key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} - restore-keys: | - target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - - name: cache ccache - uses: pat-s/always-upload-cache@v3.0.11 - with: - path: ${{ env.CCACHE_DIR }} - key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} - restore-keys: | - ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} - ccache-${{ env.CACHE_PREFIX }} - - uses: 'ros-industrial/industrial_ci@master' - env: - UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} - ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ inputs.ros_repo }} - OS_CODE_NAME: ${{ inputs.os_code_name }} - BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV }} - run: | - du -sh ${{ env.BASEDIR }}/target_ws - sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete - sudo rm -rf ${{ env.BASEDIR }}/target_ws/src - du -sh ${{ env.BASEDIR }}/target_ws diff --git a/.github/workflows/reusable-rhel-binary-build.yml b/.github/workflows/reusable-rhel-binary-build.yml deleted file mode 100644 index be4eabb76b..0000000000 --- a/.github/workflows/reusable-rhel-binary-build.yml +++ /dev/null @@ -1,69 +0,0 @@ -name: Reusable RHEL Binary Build -# Reusable action to simplify dealing with RHEL binary builds -# author: Christoph Froehlich - -on: - workflow_call: - inputs: - ros_distro: - description: 'ROS2 distribution name' - required: true - type: string - ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' - required: false - type: string - upstream_workspace: - description: 'Path to local .repos file.' - default: '' - required: false - type: string - skip_packages: - description: 'Packages to skip from build and test' - default: '' - required: false - type: string - -jobs: - rhel_binary: - name: ${{ inputs.ros_distro }} RHEL binary build - runs-on: ubuntu-latest - env: - path: src/ros2_controllers - container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-rhel - steps: - - name: Checkout default ref when build is not scheduled - if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v4 - with: - path: ${{ env.path }} - - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build - if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v4 - with: - ref: ${{ inputs.ref_for_scheduled_build }} - path: ${{ env.path }} - - name: Install dependencies - run: | - source /opt/ros/${{ inputs.ros_distro }}/setup.bash - source /opt/ros2_ws/install/local_setup.bash - if [[ -n "${{ inputs.upstream_workspace }}" ]]; then - vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} - fi - rosdep update - rosdep install -iyr --from-path src || true - - name: Build workspace - # source also underlay workspace with generate_parameter_library on rhel9 - run: | - source /opt/ros/${{ inputs.ros_distro }}/setup.bash - source /opt/ros2_ws/install/local_setup.bash - colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} - - name: Test workspace - shell: bash - continue-on-error: true - run: | - source /opt/ros/${{ inputs.ros_distro }}/setup.bash - source /opt/ros2_ws/install/local_setup.bash - colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} - colcon test-result --verbose diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml deleted file mode 100644 index 3d5bc1cf35..0000000000 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ /dev/null @@ -1,69 +0,0 @@ -name: Reusable industrial_ci Workflow with Cache -# Reusable action to simplify dealing with ROS/ROS2 industrial_ci builds with cache -# author: Denis Štogl - -on: - workflow_call: - inputs: - ros_distro: - description: 'ROS2 distribution name' - required: true - type: string - ref: - description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' - required: true - type: string - ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble.' - default: 'master' - required: false - type: string - -jobs: - reusable_ros_tooling_source_build: - name: ${{ inputs.ros_distro }} ubuntu-22.04 - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ inputs.ros_distro }} - - uses: actions/checkout@v4 - with: - ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ inputs.ros_distro }} - 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 - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.3.1 - with: - name: colcon-logs-ubuntu-22.04 - path: ros_ws/log diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml deleted file mode 100644 index 0596aeec56..0000000000 --- a/.github/workflows/rolling-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Rolling Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - master - - '*feature*' - - '*feature/**' - push: - branches: - - master - 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: rolling - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build.yml similarity index 70% rename from .github/workflows/rolling-binary-build-main.yml rename to .github/workflows/rolling-binary-build.yml index 729d5e38ba..5831f0147f 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -1,4 +1,4 @@ -name: Rolling Binary Build - main +name: Rolling Binary Build # author: Denis Štogl # description: 'Build & test all dependencies from released (binary) packages.' @@ -18,9 +18,12 @@ on: jobs: binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: rolling - ros_repo: main + ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers-not-released.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 153f4df681..cecd67603f 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -12,7 +12,7 @@ on: jobs: rolling_debian: name: Rolling debian build - uses: ./.github/workflows/reusable-debian-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master with: ros_distro: rolling upstream_workspace: ros2_controllers.rolling.repos diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml similarity index 70% rename from .github/workflows/rolling-rhel-binary-build.yml rename to .github/workflows/rolling-rhel-semi-binary-build.yml index 31c133ab69..a175d951b4 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: RHEL Rolling Binary Build +name: RHEL Rolling Semi-Binary Build on: workflow_dispatch: pull_request: @@ -10,9 +10,8 @@ on: jobs: - rolling_rhel_binary: - name: Rolling RHEL binary build - uses: ./.github/workflows/reusable-rhel-binary-build.yml + rolling_rhel: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master with: ros_distro: rolling upstream_workspace: ros2_controllers.rolling.repos diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml deleted file mode 100644 index b284c0b7d4..0000000000 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Rolling Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - pull_request: - branches: - - master - - '*feature*' - - '*feature/**' - push: - branches: - - master - 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: rolling - ros_repo: testing - upstream_workspace: ros2_controllers.rolling.repos - ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build.yml similarity index 67% rename from .github/workflows/rolling-semi-binary-build-main.yml rename to .github/workflows/rolling-semi-binary-build.yml index 206ca8bd52..edc55ebfb7 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: Rolling Semi-Binary Build - main +name: Rolling Semi-Binary Build # description: 'Build & test that compiles the main dependencies from source.' on: @@ -17,9 +17,12 @@ on: jobs: semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + matrix: + ROS_REPO: [main, testing] with: ros_distro: rolling - ros_repo: main + ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 40abcd1b0c..567b2c8ec6 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,8 +1,6 @@ name: Rolling Source Build on: workflow_dispatch: - branches: - - master push: branches: - master @@ -12,7 +10,7 @@ on: jobs: source: - uses: ./.github/workflows/reusable-ros-tooling-source-build.yml + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: rolling ref: master From 1c42c8ddcc79f8417afd87dbb9ed5821d58a3e83 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 15 Feb 2024 17:48:14 +0100 Subject: [PATCH 464/524] [CI] reviewer lottery and fail-fast (#1047) --- .github/reviewer-lottery.yml | 33 ------------------- .github/workflows/humble-binary-build.yml | 1 + .../workflows/humble-semi-binary-build.yml | 1 + .github/workflows/iron-binary-build.yml | 1 + .github/workflows/iron-semi-binary-build.yml | 1 + .github/workflows/reviewer_lottery.yml | 1 + .github/workflows/rolling-binary-build.yml | 1 + .../workflows/rolling-semi-binary-build.yml | 1 + 8 files changed, 7 insertions(+), 33 deletions(-) delete mode 100644 .github/reviewer-lottery.yml diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml deleted file mode 100644 index c6580eacd4..0000000000 --- a/.github/reviewer-lottery.yml +++ /dev/null @@ -1,33 +0,0 @@ -groups: - # Default reviewers for all pull requests. - # Usually, at least on of the maintainers should approve PR before merging. - # The best is if two maintainers do that. - - name: maintainers # name of the group - reviewers: 2 # how many reviewers do you want to assign? - internal_reviewers: 1 # how many reviewers do you want to assign when the PR author belongs to this group? - usernames: # github usernames of the reviewers - - bmagyar - - destogl - - # Reviewers group to get broader feedback. - - name: reviewers - reviewers: 5 - usernames: - - aprotyas - - arne48 - - bijoua29 - - christophfroehlich - - DasRoteSkelett - - duringhof - - erickisos - - fmauch - - jaron-l - - livanov93 - - mcbed - - moriarty - - olivier-stasse - - peterdavidfagan - - progtologist - - saikishor - - VanshGehlot - - VX792 diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 989805733f..df449caecb 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -18,6 +18,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 1d9d3bd1fb..aaed9c8ca2 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -17,6 +17,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 146d5a8c69..1510fac859 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -20,6 +20,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index ab508dfc50..38ca4fe490 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -19,6 +19,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index ed28964e01..5784b2f836 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -12,3 +12,4 @@ jobs: - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} + config: ros-controls/ros2_control_ci/.github/reviewer-lottery.yml diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 5831f0147f..d0be23f076 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -20,6 +20,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index edc55ebfb7..4784654db8 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -19,6 +19,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: From d0987700e45782df54ae966e9a9bae251cde4a85 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Sat, 17 Feb 2024 15:26:56 +0100 Subject: [PATCH 465/524] Bump pre-commit/action from 3.0.0 to 3.0.1 (#1041) Bumps [pre-commit/action](https://github.com/pre-commit/action) from 3.0.0 to 3.0.1. - [Release notes](https://github.com/pre-commit/action/releases) - [Commits](https://github.com/pre-commit/action/compare/v3.0.0...v3.0.1) --- updated-dependencies: - dependency-name: pre-commit/action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 9f090b48ca..824baf1b77 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -17,6 +17,6 @@ jobs: python-version: '3.10' - name: Install system hooks run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 with: extra_args: --all-files --hook-stage manual From f61e88b46744dd3a5231bbe94dd691ffcfd27365 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 18 Feb 2024 18:51:08 +0100 Subject: [PATCH 466/524] Update badges and add links to control.ros.org (#1050) --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 4183b38fdc..7223ceae85 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,9 @@ 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) +**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.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types From f16554c0b70f61b8a139a04f593a90232eeb5a26 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 19 Feb 2024 19:23:27 +0100 Subject: [PATCH 467/524] Fix usage of visibility macros (#1039) --- ackermann_steering_controller/CMakeLists.txt | 2 +- bicycle_steering_controller/CMakeLists.txt | 2 +- gripper_controllers/CMakeLists.txt | 4 ++++ pid_controller/CMakeLists.txt | 2 +- steering_controllers_library/CMakeLists.txt | 2 +- tricycle_controller/CMakeLists.txt | 4 +++- tricycle_steering_controller/CMakeLists.txt | 2 +- .../tricycle_steering_controller.hpp | 10 +++++----- 8 files changed, 17 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 66f09c5f09..71d0cf5f80 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_IN # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface ackermann_steering_controller.xml) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 7118e9a44d..77d82ed874 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCL # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(bicycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") +target_compile_definitions(bicycle_steering_controller PRIVATE "BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface bicycle_steering_controller.xml) diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 676062f7a3..c4a85dd453 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -45,6 +45,10 @@ target_link_libraries(gripper_action_controller PUBLIC ) ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gripper_action_controller PRIVATE "GRIPPER_ACTION_CONTROLLER_BUILDING_DLL") + pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 15e903222a..6c9e00ef8b 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -45,7 +45,7 @@ ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER__VISIBILITY_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 106fdcc464..b79236e23e 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -51,7 +51,7 @@ ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INC # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL" "_USE_MATH_DEFINES") if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 94fba916b1..ea1c4db3c1 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -47,7 +47,9 @@ target_include_directories(tricycle_controller PUBLIC ) target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(tricycle_controller PRIVATE _USE_MATH_DEFINES) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(tricycle_controller PRIVATE "TRICYCLE_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 02c9453ace..21604df0c4 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -44,7 +44,7 @@ ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INC # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(tricycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") +target_compile_definitions(tricycle_steering_controller PRIVATE "TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface tricycle_steering_controller.xml) diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 607a684df5..559de6a223 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -45,14 +45,14 @@ class TricycleSteeringController : public steering_controllers_library::Steering public: TricycleSteeringController(); - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() - override; + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( const rclcpp::Duration & period) override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() - override; + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; protected: std::shared_ptr tricycle_param_listener_; From 219a121bb0baa3d5d7278b6fab3c27d4a402dc36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 19 Feb 2024 19:25:03 +0100 Subject: [PATCH 468/524] Use reviewer lottery from ros2_control_ci (#1049) --- .github/workflows/reviewer_lottery.yml | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 5784b2f836..0584f4a7f3 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -1,15 +1,10 @@ name: Reviewer lottery +# pull_request_target takes the same events as pull_request, +# but it runs on the base branch instead of the head branch. on: pull_request_target: types: [opened, ready_for_review, reopened] jobs: - test: - runs-on: ubuntu-latest - if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' - steps: - - uses: actions/checkout@v4 - - uses: uesteibar/reviewer-lottery@v3 - with: - repo-token: ${{ secrets.GITHUB_TOKEN }} - config: ros-controls/ros2_control_ci/.github/reviewer-lottery.yml + assign_reviewers: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-reviewer-lottery.yml@master From 00172ab5dbd4948df78493e5acd40901a7227976 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 27 Feb 2024 20:07:21 +0100 Subject: [PATCH 469/524] [CI] Code coverage + pre-commit (#1057) --- .../workflows/ci-coverage-build-humble.yml | 66 ------------------- .github/workflows/ci-coverage-build-iron.yml | 66 ------------------- .github/workflows/ci-coverage-build.yml | 66 ------------------- .github/workflows/ci-format.yml | 22 ------- .github/workflows/ci-ros-lint.yml | 64 ------------------ .github/workflows/humble-coverage-build.yml | 17 +++++ .github/workflows/humble-pre-commit.yml | 14 ++++ .github/workflows/iron-coverage-build.yml | 17 +++++ .github/workflows/iron-pre-commit.yml | 14 ++++ .github/workflows/rolling-coverage-build.yml | 17 +++++ .github/workflows/rolling-pre-commit.yml | 14 ++++ .github/workflows/update-pre-commit.yml | 12 ++++ .pre-commit-config.yaml | 49 ++++++++------ .../joint_trajectory_controller.py | 8 +-- .../update_combo.py | 2 +- rqt_joint_trajectory_controller/setup.py | 14 ++++ 16 files changed, 153 insertions(+), 309 deletions(-) delete mode 100644 .github/workflows/ci-coverage-build-humble.yml delete mode 100644 .github/workflows/ci-coverage-build-iron.yml delete mode 100644 .github/workflows/ci-coverage-build.yml delete mode 100644 .github/workflows/ci-format.yml delete mode 100644 .github/workflows/ci-ros-lint.yml create mode 100644 .github/workflows/humble-coverage-build.yml create mode 100644 .github/workflows/humble-pre-commit.yml create mode 100644 .github/workflows/iron-coverage-build.yml create mode 100644 .github/workflows/iron-pre-commit.yml create mode 100644 .github/workflows/rolling-coverage-build.yml create mode 100644 .github/workflows/rolling-pre-commit.yml create mode 100644 .github/workflows/update-pre-commit.yml diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml deleted file mode 100644 index 357ee3dfa8..0000000000 --- a/.github/workflows/ci-coverage-build-humble.yml +++ /dev/null @@ -1,66 +0,0 @@ -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.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - 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 - pid_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@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.1 - 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 deleted file mode 100644 index 7914a1acb0..0000000000 --- a/.github/workflows/ci-coverage-build-iron.yml +++ /dev/null @@ -1,66 +0,0 @@ -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.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - 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 - pid_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@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.1 - 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 deleted file mode 100644 index b96276ca5a..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,66 +0,0 @@ -name: Coverage Build -on: - workflow_dispatch: - push: - branches: - - master - pull_request: - branches: - - master - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: rolling - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - 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 - pid_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@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.1 - with: - name: colcon-logs-coverage-rolling - path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml deleted file mode 100644 index 824baf1b77..0000000000 --- a/.github/workflows/ci-format.yml +++ /dev/null @@ -1,22 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - pull_request: - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5.0.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.1 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index df17d11dc4..0000000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,64 +0,0 @@ -name: ROS Lint -on: - pull_request: - -env: - 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 - pid_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 - -jobs: - ament_lint: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - env: - AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.1 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: ${{ env.package-name }} - - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml new file mode 100644 index 0000000000..0910572227 --- /dev/null +++ b/.github/workflows/humble-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + pull_request: + branches: + - humble + +jobs: + coverage_humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml new file mode 100644 index 0000000000..be8c84b05b --- /dev/null +++ b/.github/workflows/humble-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Humble + +on: + workflow_dispatch: + pull_request: + branches: + - humble + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml new file mode 100644 index 0000000000..d82c52bf51 --- /dev/null +++ b/.github/workflows/iron-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Iron +on: + workflow_dispatch: + push: + branches: + - iron + pull_request: + branches: + - iron + +jobs: + coverage_iron: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml new file mode 100644 index 0000000000..60ad26d073 --- /dev/null +++ b/.github/workflows/iron-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Iron + +on: + workflow_dispatch: + pull_request: + branches: + - iron + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml new file mode 100644 index 0000000000..4d4750c54c --- /dev/null +++ b/.github/workflows/rolling-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Rolling +on: + workflow_dispatch: + push: + branches: + - master + pull_request: + branches: + - master + +jobs: + coverage_rolling: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml new file mode 100644 index 0000000000..9c87311bd7 --- /dev/null +++ b/.github/workflows/rolling-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml new file mode 100644 index 0000000000..8b9545dff1 --- /dev/null +++ b/.github/workflows/update-pre-commit.yml @@ -0,0 +1,12 @@ +name: Auto Update pre-commit +# Update pre-commit config and create PR if changes are detected +# author: Christoph Fröhlich + +on: + workflow_dispatch: + schedule: + - cron: '0 0 * * 0' # Run every Sunday at midnight + +jobs: + auto_update_and_create_pr: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 32d194d73d..6da427c6ee 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,3 +1,4 @@ + # To use: # # pre-commit run -a @@ -15,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-ast @@ -29,57 +30,57 @@ repos: - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace + exclude_types: [rst] - id: fix-byte-order-marker + # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.15.1 hooks: - id: pyupgrade args: [--py36-plus] - - repo: https://github.com/psf/black - rev: 22.12.0 - hooks: - - id: black - args: ["--line-length=99"] - # PyDocStyle - repo: https://github.com/PyCQA/pydocstyle - rev: 6.2.2 + rev: 6.3.0 hooks: - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - repo: https://github.com/psf/black + rev: 24.2.0 + hooks: + - id: black + args: ["--line-length=99"] + - repo: https://github.com/pycqa/flake8 - rev: 6.0.0 + rev: 7.0.0 hooks: - id: flake8 - args: ["--ignore=E501"] + args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v17.0.6 hooks: - id: clang-format + args: ['-fallback-style=none', '-i'] - repo: local hooks: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - repo: local hooks: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -91,7 +92,6 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -102,7 +102,6 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] entry: ament_copyright language: system @@ -125,8 +124,18 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.6 hooks: - id: codespell - args: ['--write-changes', '--uri-ignore-words-list=ist'] + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.28.0 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 162977cdfe..72c11625e4 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -214,9 +214,9 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) - except (ValueError): + except ValueError: pass - except (ValueError): + except ValueError: pass # def trigger_configuration(self): @@ -427,7 +427,7 @@ def _update_cmd_cb(self): cmd = pos try: cmd = self._joint_pos[name]["command"] - except (KeyError): + except KeyError: pass max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) @@ -445,7 +445,7 @@ def _update_joint_widgets(self): try: joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) - except (KeyError): + except KeyError: pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index f0b632322f..6f736910e2 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -36,7 +36,7 @@ def update_combo(combo, new_vals): selected_id = -1 try: selected_id = new_vals.index(selected_val) - except (ValueError): + except ValueError: combo.setCurrentIndex(-1) # Re-populate items diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 8bcb07ac54..adb3f94668 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2024 Apache License, Version 2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from setuptools import setup from glob import glob From 7d38fb9a8c67bc996f12821280c3550c33069602 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Mar 2024 10:37:43 +0100 Subject: [PATCH 470/524] Use CMake target for eigen (#1058) --- admittance_controller/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index b2c10e0ba5..10ca5caa40 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,7 +11,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs control_toolbox controller_interface - Eigen3 generate_parameter_library geometry_msgs hardware_interface @@ -34,6 +33,7 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(Eigen3 REQUIRED NO_MODULE) generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml @@ -49,6 +49,7 @@ target_include_directories(admittance_controller PUBLIC ) target_link_libraries(admittance_controller PUBLIC admittance_controller_parameters + Eigen3::Eigen ) ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) From 61617b3f58a2d5915d9ea1121fb1cb362891a46e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 2 Mar 2024 20:55:12 +0100 Subject: [PATCH 471/524] [JTC] Parse URDF for continuous joints (#949) --- joint_trajectory_controller/CMakeLists.txt | 1 + .../doc/parameters_context.yaml | 7 +- .../joint_trajectory_controller.hpp | 3 + joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 43 ++- ...oint_trajectory_controller_parameters.yaml | 7 +- .../test/test_assets.hpp | 278 ++++++++++++++++++ .../test/test_trajectory_controller.cpp | 28 +- .../test/test_trajectory_controller_utils.hpp | 24 +- 9 files changed, 360 insertions(+), 32 deletions(-) create mode 100644 joint_trajectory_controller/test/test_assets.hpp diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index c92d61b3f3..4b0ca82df8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs + urdf ) find_package(ament_cmake REQUIRED) diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 2ffe8aed36..4e4dacf202 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -9,5 +9,10 @@ gains: | .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + + If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`, + i.e., the shortest rotation to the target position is the desired motion. + 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. 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 111837cc17..ae370df0a6 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 @@ -43,6 +43,7 @@ #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "urdf/model.h" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" @@ -298,6 +299,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + urdf::Model model_; + /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 5aad4cb86e..8457c02aeb 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -25,6 +25,7 @@ rsl tl_expected trajectory_msgs + urdf ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 46544bf875..6cae29e083 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -36,6 +36,7 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "urdf/model.h" namespace joint_trajectory_controller { @@ -46,6 +47,23 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { + if (!urdf_.empty()) + { + if (!model_.initString(urdf_)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); + } + } + else + { + // empty URDF is used for some tests + RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); + } + try { // Create the parameter listener and get the parameters @@ -684,12 +702,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization from ROS parameters (angle_wraparound) + // Configure joint position error normalization (angle_wraparound) joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - joints_angle_wraparound_[i] = gains.angle_wraparound; + if (gains.angle_wraparound) + { + // TODO(christophfroehlich): remove this warning in a future release (ROS-J) + RCLCPP_WARN( + logger, + "[Deprecated] Parameter 'gains..angle_wraparound' is deprecated. The " + "angle_wraparound is now used if a continuous joint is configured in the URDF."); + joints_angle_wraparound_[i] = true; + } + + if (!urdf_.empty()) + { + auto urdf_joint = model_.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + logger, "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } } if (params_.state_interfaces.empty()) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ded5bb7ca3..b17229cab8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -125,11 +125,8 @@ joint_trajectory_controller: angle_wraparound: { type: bool, default_value: false, - description: 'For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - 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.' + description: "[deprecated] For joints that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." } constraints: stopped_velocity_tolerance: { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp new file mode 100644 index 0000000000..ccdbaf4c8a --- /dev/null +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -0,0 +1,278 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ASSETS_HPP_ +#define TEST_ASSETS_HPP_ + +#include + +namespace test_trajectory_controllers +{ +const auto urdf_rrrbot_revolute = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_rrrbot_continuous = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace test_trajectory_controllers + +#endif // TEST_ASSETS_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2bfe6f150d..67eb959df2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -45,6 +45,7 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_assets.hpp" #include "test_trajectory_controller_utils.hpp" using lifecycle_msgs::msg::State; @@ -463,14 +464,15 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) const double EPS = 1e-6; /** - * @brief check if calculated trajectory error is correct with angle wraparound=true + * @brief check if calculated trajectory error is correct (angle wraparound) for continuous joints */ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) { rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -554,14 +556,15 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru } /** - * @brief check if calculated trajectory error is correct with angle wraparound=false + * @brief check if calculated trajectory error is correct (no angle wraparound) for revolute joints */ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) { rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); size_t n_joints = joint_names_.size(); @@ -636,15 +639,16 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal } /** - * @brief check if position error of revolute joints are wrapped around if not configured so + * @brief check if position error of revolute joints aren't wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - bool angle_wraparound = false; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -737,14 +741,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun } /** - * @brief check if position error of revolute joints are wrapped around if configured so + * @brief check if position error of continuous joints are wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -1844,7 +1850,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co 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, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from state interfaces @@ -1888,7 +1894,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from command interfaces diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6978d0e452..baa4111239 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -215,9 +215,10 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpTrajectoryController( - rclcpp::Executor & executor, const std::vector & parameters = {}) + rclcpp::Executor & executor, const std::vector & parameters = {}, + const std::string & urdf = "") { - auto ret = SetUpTrajectoryControllerLocal(parameters); + auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) { FAIL(); @@ -226,7 +227,7 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}) + const std::vector & parameters = {}, const std::string & urdf = "") { traj_controller_ = std::make_shared(); @@ -241,11 +242,10 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); } - void SetPidParameters( - double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) + void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -258,20 +258,18 @@ 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_value); - const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_value); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } 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, 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) + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, + const std::string & urdf = "") { auto has_nonzero_vel_param = std::find_if( @@ -287,10 +285,10 @@ class TrajectoryControllerTest : public ::testing::Test parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } // read-only parameters have to be set before init -> won't be read otherwise - SetUpTrajectoryController(executor, parameters_local); + SetUpTrajectoryController(executor, parameters_local, urdf); // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); + SetPidParameters(k_p, ff); traj_controller_->get_node()->configure(); ActivateTrajectoryController( From 0b432919aca596fd66ae413a864500e94f64d5e3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Mar 2024 12:57:02 +0100 Subject: [PATCH 472/524] added conditioning to have rolling tags compilable in older versions (#1071) --- .../include/diff_drive_controller/odometry.hpp | 10 ++++++++++ .../src/joint_state_broadcaster.cpp | 1 - .../src/joint_trajectory_controller.cpp | 1 - .../test/test_trajectory_controller.cpp | 1 - pid_controller/src/pid_controller.cpp | 14 ++++++++++++++ .../src/range_sensor_broadcaster.cpp | 3 +++ .../test/test_range_sensor_broadcaster.cpp | 10 ++++++++++ .../steering_odometry.hpp | 16 ++++++++++++++-- .../src/steering_odometry.cpp | 4 ++-- .../include/tricycle_controller/odometry.hpp | 10 ++++++++++ 10 files changed, 63 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 83ab270f72..edca431d3d 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -25,7 +25,12 @@ #include #include "rclcpp/time.hpp" +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace diff_drive_controller { @@ -50,7 +55,12 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 3c2192d40e..a53fe2b3c4 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -24,7 +24,6 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/clock.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6cae29e083..5e3d9d1c7d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -29,7 +29,6 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 67eb959df2..f5e9cb7260 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -32,7 +32,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/node.hpp" diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 05fee986dd..19cebbde4e 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -30,10 +30,24 @@ namespace { // utility // Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 rclcpp::QoS qos_services = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) .reliable() .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index b821da8c13..7c6d714be3 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -79,7 +79,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( realtime_publisher_->msg_.field_of_view = params_.field_of_view; realtime_publisher_->msg_.min_range = params_.min_range; realtime_publisher_->msg_.max_range = params_.max_range; +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if SENSOR_MSGS_VERSION_MAJOR >= 5 realtime_publisher_->msg_.variance = params_.variance; +#endif realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 010f18c1a6..a23d5e3cde 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -203,7 +203,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) @@ -224,7 +226,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif sensor_range_ = 4.0; subscribe_and_get_message(range_msg); @@ -235,7 +239,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success) @@ -257,7 +263,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif sensor_range_ = 6.0; subscribe_and_get_message(range_msg); @@ -269,7 +277,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } int main(int argc, char ** argv) 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 95bcef7e63..e4a22f6d3b 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -24,7 +24,12 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace steering_odometry { @@ -229,6 +234,13 @@ class SteeringOdometry */ void reset_accumulators(); +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif + /// Current timestamp: rclcpp::Time timestamp_; @@ -256,8 +268,8 @@ class SteeringOdometry double traction_left_wheel_old_pos_; /// Rolling mean accumulators for the linear and angular velocities: size_t velocity_rolling_window_size_; - rcpputils::RollingMeanAccumulator linear_acc_; - rcpputils::RollingMeanAccumulator angular_acc_; + RollingMeanAccumulator linear_acc_; + RollingMeanAccumulator angular_acc_; }; } // namespace steering_odometry diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index e2ced036ff..aadd047f2e 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -324,8 +324,8 @@ void SteeringOdometry::integrate_exact(double linear, double angular) void SteeringOdometry::reset_accumulators() { - linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); - angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); } } // namespace steering_odometry diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 62eb51d6cc..13d65a980a 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -22,7 +22,12 @@ #include #include "rclcpp/time.hpp" +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace tricycle_controller { @@ -45,7 +50,12 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); From 28cb552903a9d17264594efeb148f8b04bda3039 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 11 Mar 2024 13:02:46 +0000 Subject: [PATCH 473/524] Bump version of pre-commit hooks (#1073) --- .pre-commit-config.yaml | 2 +- .../src/joint_trajectory_controller.cpp | 6 ++---- .../test/test_trajectory_controller_utils.hpp | 7 +++---- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6da427c6ee..dc3b3e837d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v17.0.6 + rev: v18.1.0 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e3d9d1c7d..4a4b63cb75 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -444,8 +444,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -515,8 +514,7 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index baa4111239..f049bc65a9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -273,10 +273,9 @@ class TrajectoryControllerTest : public ::testing::Test { auto has_nonzero_vel_param = std::find_if( - parameters.begin(), parameters.end(), - [](const rclcpp::Parameter & param) { - return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; - }) != parameters.end(); + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; }) != + parameters.end(); std::vector parameters_local = parameters; if (!has_nonzero_vel_param) From ab1e042585d42cb36745fbf84d66144d529c1158 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Mar 2024 07:15:23 +0100 Subject: [PATCH 474/524] Remove action_msg dependency (#1077) --- joint_trajectory_controller/test/test_trajectory_actions.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 87d557df1b..332a30c53a 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -27,7 +27,6 @@ #include #include -#include "action_msgs/msg/goal_status_array.hpp" #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" From 5a28a2aea9ef76cf32091e43cb7c72461f624ad4 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 18 Mar 2024 06:23:52 +0000 Subject: [PATCH 475/524] Bump version of pre-commit hooks (#1079) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index dc3b3e837d..31ba833ba5 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -49,7 +49,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.2.0 + rev: 24.3.0 hooks: - id: black args: ["--line-length=99"] @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.0 + rev: v18.1.1 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] From 0c2c458abc8c8e9d547f88661871e036b8eb2437 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 21 Mar 2024 02:27:53 -0400 Subject: [PATCH 476/524] Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (#1084) --- ackermann_steering_controller/package.xml | 1 + bicycle_steering_controller/package.xml | 1 + pid_controller/package.xml | 1 + pid_controller/src/pid_controller.cpp | 3 +++ tricycle_steering_controller/package.xml | 1 + 5 files changed, 7 insertions(+) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 512af88534..a3183af262 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index bc560d9bf7..6507700a5d 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 9cda85aa03..c1106a0b23 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -14,6 +14,7 @@ generate_parameter_library angles + backward_ros control_msgs control_toolbox controller_interface diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 19cebbde4e..b76926d5a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -26,6 +26,9 @@ #include "control_msgs/msg/single_dof_state.hpp" #include "controller_interface/helpers.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/version.h" + namespace { // utility diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index bed6e451fa..991aca2c05 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -18,6 +18,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface From 3520e850feec4df94435ad27bd1ef92197f103f2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 22 Mar 2024 12:26:17 +0000 Subject: [PATCH 477/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ pid_controller/CHANGELOG.rst | 7 +++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 99 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 3b77b7b78f..1ea0329f26 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 65db88f462..a611753f0f 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use CMake target for eigen (`#1058 `_) +* Contributors: Christoph Fröhlich + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 16192d9f8a..09cd7e7c6e 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1ec4329fca..63e4226d4f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` also for diff_drive (`#1021 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ac47c3c2b3..e9cd8953da 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index c61f166b8e..0bae0e0cbc 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54a428ec3f..07f48d32e4 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 76ea9418d2..9509b8858e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d906fb897f..28e00cdfa2 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1444f5f8ab..2580a0f0e6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 17b26ad551..2c0467fefb 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove action_msg dependency (`#1077 `_) +* Bump version of pre-commit hooks (`#1073 `_) +* Added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Parse URDF for continuous joints (`#949 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, github-actions[bot] + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 7d5d248576..7948c7f915 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 6c55f59602..eb02c1f499 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 62c87c8717..7d5dedab61 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index fc8ca62020..bcb3d11572 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 41a20f3841..a26663e26e 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index cd4ae0809e..cc6f7d1313 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Code coverage + pre-commit (`#1057 `_) +* Contributors: Christoph Fröhlich + 4.6.0 (2024-02-12) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e8288f1955..f05e310412 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cf2a599b20..c37f7537a7 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 718df4e014..35dd1b27df 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e6689b23a8..8c8970b704 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) From d77a0011f200da524e1a19a2f4b3a4c0ff688809 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 22 Mar 2024 12:26:44 +0000 Subject: [PATCH 478/524] 4.7.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 1ea0329f26..f14bd8ba01 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) * Fix usage of visibility macros (`#1039 `_) * Contributors: Chris Lalancette, Silvio Traversaro diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index a3183af262..771494e67f 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.6.0 + 4.7.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 a611753f0f..7dd8e0b321 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Use CMake target for eigen (`#1058 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2c50dde2e7..e9779eda4d 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.6.0 + 4.7.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 09cd7e7c6e..de67c5bc72 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) * Fix usage of visibility macros (`#1039 `_) * Contributors: Chris Lalancette, Silvio Traversaro diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 6507700a5d..97c12290df 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.6.0 + 4.7.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 63e4226d4f..ae38961d10 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index b0e818dd50..5252803de9 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.6.0 + 4.7.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 e9cd8953da..306cc24ff6 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 52165f6ec1..9acfdd2ddc 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.6.0 + 4.7.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 0bae0e0cbc..f4cbbe1bec 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 178a90d5cf..dee37dd565 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.6.0 + 4.7.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 07f48d32e4..607ec25f23 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 9b6c2559f4..1aed0d3c83 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.6.0 + 4.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 9509b8858e..bb4d699bd0 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Fix usage of visibility macros (`#1039 `_) * Contributors: Silvio Traversaro diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a6b83b43ec..7183bc7ebd 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.6.0 + 4.7.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 28e00cdfa2..6a9df05038 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5a4ca12e87..3817b7a536 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.6.0 + 4.7.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 2580a0f0e6..9762bcb55c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 0bf3db58dd..14f226450e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.6.0 + 4.7.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 2c0467fefb..d008380af9 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Remove action_msg dependency (`#1077 `_) * Bump version of pre-commit hooks (`#1073 `_) * Added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 8457c02aeb..a44d55a731 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.6.0 + 4.7.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 7948c7f915..82e631a75c 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) * Added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Fix usage of visibility macros (`#1039 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index c1106a0b23..151862dc11 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.6.0 + 4.7.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index eb02c1f499..cdb4165691 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 13e80f4d99..b56e3e154a 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.6.0 + 4.7.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 7d5dedab61..89365b7754 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3bbe1d2130..4cdae222a5 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.6.0 + 4.7.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 bcb3d11572..2d7e5f08de 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 10ae5427e9..fc2e40fb8a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.6.0 + 4.7.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 a26663e26e..a77c2635e8 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index fb5b3d8bcf..fef330a0c7 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.6.0 + 4.7.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 9219fea906..273957f688 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.6.0", + version="4.7.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 cc6f7d1313..416c399cc6 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * [CI] Code coverage + pre-commit (`#1057 `_) * Contributors: Christoph Fröhlich diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 107d50681c..c7ec8d4666 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.6.0 + 4.7.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 adb3f94668..0339f47b5a 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.6.0", + version="4.7.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 f05e310412..43e3cc30da 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Fix usage of visibility macros (`#1039 `_) * Contributors: Sai Kishor Kothakota, Silvio Traversaro diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 8212f58d91..97d03dc809 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.6.0 + 4.7.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 c37f7537a7..150a575260 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) * Fix usage of visibility macros (`#1039 `_) * Contributors: Sai Kishor Kothakota, Silvio Traversaro diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 04995732f2..900f906a11 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.6.0 + 4.7.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 35dd1b27df..04e3bf43fe 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) * Fix usage of visibility macros (`#1039 `_) * Contributors: Chris Lalancette, Silvio Traversaro diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 991aca2c05..ccb973be40 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.6.0 + 4.7.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 8c8970b704..e15ecc94e7 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.7.0 (2024-03-22) +------------------ 4.6.0 (2024-02-12) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ff403e178f..a6c28a3cd1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.6.0 + 4.7.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 689efe716fd97f4c79638689ececc65fe175b418 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 27 Mar 2024 19:23:54 +0100 Subject: [PATCH 479/524] add missing compiler definitions of RCPPUTILS_VERSION (#1089) --- ackermann_steering_controller/CMakeLists.txt | 2 ++ bicycle_steering_controller/CMakeLists.txt | 2 ++ diff_drive_controller/CMakeLists.txt | 2 ++ steering_controllers_library/CMakeLists.txt | 2 ++ tricycle_controller/CMakeLists.txt | 2 ++ tricycle_steering_controller/CMakeLists.txt | 2 ++ 6 files changed, 12 insertions(+) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 71d0cf5f80..a34be3c672 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 77d82ed874..6535a73260 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(bicycle_steering_controller_parameters src/bicycle_steering_controller.yaml diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index d67815b5e0..70a54e302c 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -25,6 +25,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index b79236e23e..e2bfdbab71 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -30,6 +30,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(steering_controllers_library_parameters src/steering_controllers_library.yaml diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index ea1c4db3c1..e264c1f258 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -29,6 +29,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(tricycle_controller_parameters src/tricycle_controller_parameter.yaml diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 21604df0c4..92c2782092 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(tricycle_steering_controller_parameters src/tricycle_steering_controller.yaml From 9e97c00496c239bf4961dc043a3f461aae9b23b8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Apr 2024 11:39:54 +0200 Subject: [PATCH 480/524] [JTC] Remove unused test code (#1095) --- .../test/test_trajectory_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f5e9cb7260..0b00bd5380 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -51,8 +51,6 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } - TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -60,8 +58,6 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - // const auto future_handle_ = std::async(std::launch::async, spin, &executor); - const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); From d51306a490328489384dc5468ea1c223792e1660 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 28 Apr 2024 09:37:04 +0100 Subject: [PATCH 481/524] Bump version of pre-commit hooks (#1087) --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 31ba833ba5..a34629da31 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v4.6.0 hooks: - id: check-added-large-files - id: check-ast @@ -36,7 +36,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.15.1 + rev: v3.15.2 hooks: - id: pyupgrade args: [--py36-plus] @@ -49,7 +49,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.3.0 + rev: 24.4.2 hooks: - id: black args: ["--line-length=99"] @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.1 + rev: v18.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -131,7 +131,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.0 + rev: 0.28.2 hooks: - id: check-github-workflows args: ["--verbose"] From edcb7e0fec676f337ac5da1d3270b5a6009d4669 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Apr 2024 14:51:44 +0200 Subject: [PATCH 482/524] [PID] Add example yaml to docs (#951) * Add default yaml to docs * Add examples from test folder --- pid_controller/doc/userdoc.rst | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index 5570f523fe..fc94357ab3 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -83,4 +83,21 @@ Parameters The PID controller uses the `generate_parameter_library `_ to handle its parameters. +List of parameters +========================= .. generate_parameter_library_details:: ../src/pid_controller.yaml + + +An example parameter file +========================= + + +An example parameter file for this controller can be found in `the test folder (standalone) `_: + +.. literalinclude:: ../test/pid_controller_params.yaml + :language: yaml + +or as `preceding controller `_: + +.. literalinclude:: ../test/pid_controller_preceding_params.yaml + :language: yaml From fb927bb9f1d3e4277ef31aebbe5f13aabb33dded Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Apr 2024 17:42:20 +0200 Subject: [PATCH 483/524] Add migration notes for Jazzy (#1092) * Add migration notes for Jazzy * Split migration notes from release notes * Add missing github_url * Rename file * Rename file * Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --------- Co-authored-by: Sai Kishor Kothakota --- doc/migration/Jazzy.rst | 20 ++++++++++++++++ doc/release_notes/Jazzy.rst | 48 +++++++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 doc/migration/Jazzy.rst create mode 100644 doc/release_notes/Jazzy.rst diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst new file mode 100644 index 0000000000..72e6a52e6d --- /dev/null +++ b/doc/migration/Jazzy.rst @@ -0,0 +1,20 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst + +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. + + +diff_drive_controller +***************************** +* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). + +joint_trajectory_controller +***************************** + +* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). +* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst new file mode 100644 index 0000000000..8ecac3508f --- /dev/null +++ b/doc/release_notes/Jazzy.rst @@ -0,0 +1,48 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst + +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes the changes between Iron (previous) and Jazzy (current) releases. + +admittance_controller +************************ +* Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 `_). + +diff_drive_controller +***************************** +* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). +* Remove unused parameter ``wheels_per_side`` (`#958 `_). + +joint_trajectory_controller +***************************** + +* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). +* Activate update of dynamic parameters (`#761 `_ and `#849 `_). +* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). +* Continue with last trajectory-point on success, instead of hold-position from current state (`#842 `_). +* Add console output for tolerance checks (`#932 `_): + + .. code:: + + [tolerances]: State tolerances failed for joint 2: + [tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000 + [trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds + +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* Action field ``error_string`` is now filled with meaningful strings (`#887 `_). +* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). + +pid_controller +************************ +* 🚀 The PID controller was added 🎉 (`#434 `_). + +steering_controllers_library +******************************** +* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). +* A fix for Ackermann steering odometry was added (`#921 `_). + +tricycle_controller +************************ +* tricycle_controller now uses generate_parameter_library (`#957 `_). From 2a59f60a4827d4f3f0817991a4dfdf4a65d91037 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Apr 2024 21:09:22 +0200 Subject: [PATCH 484/524] [CI] Specify runner/container images (#1105) --- .github/workflows/humble-source-build.yml | 1 + .github/workflows/iron-source-build.yml | 1 + .github/workflows/rolling-coverage-build.yml | 2 +- .github/workflows/rolling-pre-commit.yml | 2 +- .github/workflows/rolling-source-build.yml | 1 + 5 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 7b4427d6d6..6325fd50d8 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -15,3 +15,4 @@ jobs: ros_distro: humble ref: humble ros2_repo_branch: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 3609dcfc41..2d12734df0 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -15,3 +15,4 @@ jobs: ros_distro: iron ref: iron ros2_repo_branch: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 4d4750c54c..8374afe8dc 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -14,4 +14,4 @@ jobs: secrets: inherit with: ros_distro: rolling - os_name: ubuntu-22.04 + container: ubuntu:24.04 diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 9c87311bd7..4d91d19100 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -11,4 +11,4 @@ jobs: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master with: ros_distro: rolling - os_name: ubuntu-22.04 + container: ubuntu:24.04 diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 567b2c8ec6..ae38197455 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -15,3 +15,4 @@ jobs: ros_distro: rolling ref: master ros2_repo_branch: rolling + container: ubuntu:24.04 From 4ae8dafb65f4f3d4c4bd425118bfe9022863cc28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 8 May 2024 13:35:36 +0200 Subject: [PATCH 485/524] [CI] Add jazzy and other minor updates (#1114) * Add file type filters * Add jazzy workflows and readme * Check docs on correct branch * Specify workflow names * Run auto-update only once per month * Update upstream pre-commit wf --------- Co-authored-by: Felix Exner (fexner) --- .../workflows/humble-abi-compatibility.yml | 9 ++++ .github/workflows/humble-binary-build.yml | 18 +++++++ .github/workflows/humble-check-docs.yml | 18 +++++++ .github/workflows/humble-coverage-build.yml | 21 +++++++- .github/workflows/humble-debian-build.yml | 9 ++++ .github/workflows/humble-pre-commit.yml | 1 - .../humble-rhel-semi-binary-build.yml | 9 ++++ .../workflows/humble-semi-binary-build.yml | 18 +++++++ .github/workflows/humble-source-build.yml | 8 ++++ .github/workflows/iron-abi-compatibility.yml | 9 ++++ .github/workflows/iron-binary-build.yml | 18 +++++++ .github/workflows/iron-check-docs.yml | 18 +++++++ .github/workflows/iron-coverage-build.yml | 21 +++++++- .github/workflows/iron-debian-build.yml | 9 ++++ .github/workflows/iron-pre-commit.yml | 1 - .../workflows/iron-rhel-semi-binary-build.yml | 9 ++++ .github/workflows/iron-semi-binary-build.yml | 18 +++++++ .github/workflows/iron-source-build.yml | 9 ++++ .github/workflows/jazzy-abi-compatibility.yml | 27 +++++++++++ .github/workflows/jazzy-binary-build.yml | 48 +++++++++++++++++++ .github/workflows/jazzy-check-docs.yml | 18 +++++++ .github/workflows/jazzy-coverage-build.yml | 41 ++++++++++++++++ .github/workflows/jazzy-debian-build.yml | 29 +++++++++++ .github/workflows/jazzy-pre-commit.yml | 13 +++++ .../jazzy-rhel-semi-binary-build.yml | 28 +++++++++++ .github/workflows/jazzy-semi-binary-build.yml | 47 ++++++++++++++++++ .github/workflows/jazzy-source-build.yml | 27 +++++++++++ .../workflows/rolling-abi-compatibility.yml | 9 ++++ .github/workflows/rolling-binary-build.yml | 17 +++++++ ...-check-docs.yml => rolling-check-docs.yml} | 8 +++- .github/workflows/rolling-coverage-build.yml | 21 +++++++- .github/workflows/rolling-debian-build.yml | 9 ++++ .github/workflows/rolling-pre-commit.yml | 1 - .../rolling-rhel-semi-binary-build.yml | 9 ++++ .../workflows/rolling-semi-binary-build.yml | 18 +++++++ .github/workflows/rolling-source-build.yml | 9 ++++ .github/workflows/update-pre-commit.yml | 2 +- README.md | 3 +- ros2_controllers-not-released.jazzy.repos | 5 ++ ros2_controllers.jazzy.repos | 25 ++++++++++ 40 files changed, 628 insertions(+), 9 deletions(-) create mode 100644 .github/workflows/humble-check-docs.yml create mode 100644 .github/workflows/iron-check-docs.yml create mode 100644 .github/workflows/jazzy-abi-compatibility.yml create mode 100644 .github/workflows/jazzy-binary-build.yml create mode 100644 .github/workflows/jazzy-check-docs.yml create mode 100644 .github/workflows/jazzy-coverage-build.yml create mode 100644 .github/workflows/jazzy-debian-build.yml create mode 100644 .github/workflows/jazzy-pre-commit.yml create mode 100644 .github/workflows/jazzy-rhel-semi-binary-build.yml create mode 100644 .github/workflows/jazzy-semi-binary-build.yml create mode 100644 .github/workflows/jazzy-source-build.yml rename .github/workflows/{ci-check-docs.yml => rolling-check-docs.yml} (67%) create mode 100644 ros2_controllers-not-released.jazzy.repos create mode 100644 ros2_controllers.jazzy.repos diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 5c288fabfb..d72d3725b3 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-abi-compatibility.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' jobs: abi_check: diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index df449caecb..a78f6135b6 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -7,9 +7,27 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml new file mode 100644 index 0000000000..0eeeaedf40 --- /dev/null +++ b/.github/workflows/humble-check-docs.yml @@ -0,0 +1,18 @@ +name: Humble Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@humble + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index 0910572227..f39758a512 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -4,9 +4,29 @@ on: push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' jobs: coverage_humble: @@ -14,4 +34,3 @@ jobs: secrets: inherit with: ros_distro: humble - os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index db0a8456f8..b018fcea84 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-debian-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index be8c84b05b..5bb2408578 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -11,4 +11,3 @@ jobs: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master with: ros_distro: humble - os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index ccf64a0246..2a7814f8b3 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-rhel-semi-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index aaed9c8ca2..72287fa115 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -6,9 +6,27 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-semi-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-semi-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 6325fd50d8..44d474ded4 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -4,6 +4,14 @@ on: push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/humble-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index ab6642625f..be1aec6680 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/iron-abi-compatibility.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' jobs: abi_check: diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 1510fac859..802ca97bb8 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -9,9 +9,27 @@ on: - iron - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/iron-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/iron-binary-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml new file mode 100644 index 0000000000..f842902055 --- /dev/null +++ b/.github/workflows/iron-check-docs.yml @@ -0,0 +1,18 @@ +name: Iron Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - iron + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml index d82c52bf51..bbc9c4e975 100644 --- a/.github/workflows/iron-coverage-build.yml +++ b/.github/workflows/iron-coverage-build.yml @@ -4,9 +4,29 @@ on: push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' jobs: coverage_iron: @@ -14,4 +34,3 @@ jobs: secrets: inherit with: ros_distro: iron - os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index e56e8940ad..105eb29aba 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml index 60ad26d073..a128958031 100644 --- a/.github/workflows/iron-pre-commit.yml +++ b/.github/workflows/iron-pre-commit.yml @@ -11,4 +11,3 @@ jobs: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master with: ros_distro: iron - os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-rhel-semi-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml index 66ad427a98..109dac7550 100644 --- a/.github/workflows/iron-rhel-semi-binary-build.yml +++ b/.github/workflows/iron-rhel-semi-binary-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 38ca4fe490..bf5edcaa56 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -8,9 +8,27 @@ on: - iron - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 2d12734df0..c460762259 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -4,6 +4,15 @@ on: push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml new file mode 100644 index 0000000000..8e557b5ee3 --- /dev/null +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -0,0 +1,27 @@ +name: Jazzy - ABI Compatibility Check +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: jazzy + ROS_REPO: testing + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml new file mode 100644 index 0000000000..6ec46eb6c1 --- /dev/null +++ b/.github/workflows/jazzy-binary-build.yml @@ -0,0 +1,48 @@ +name: Jazzy Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: jazzy + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.jazzy.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml new file mode 100644 index 0000000000..2f9eaf18bd --- /dev/null +++ b/.github/workflows/jazzy-check-docs.yml @@ -0,0 +1,18 @@ +name: Jazzy Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@jazzy + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml new file mode 100644 index 0000000000..c96ac88c32 --- /dev/null +++ b/.github/workflows/jazzy-coverage-build.yml @@ -0,0 +1,41 @@ +name: Coverage Build - Jazzy +on: + workflow_dispatch: + # TODO(anyone) activate when branched for Jazzy + # push: + # branches: + # - master + # paths: + # - '**.hpp' + # - '**.h' + # - '**.cpp' + # - '.github/workflows/jazzy-coverage-build.yml' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + # pull_request: + # branches: + # - master + # paths: + # - '**.hpp' + # - '**.h' + # - '**.cpp' + # - '.github/workflows/jazzy-coverage-build.yml' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + +jobs: + coverage_jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml new file mode 100644 index 0000000000..61a47d0f2e --- /dev/null +++ b/.github/workflows/jazzy-debian-build.yml @@ -0,0 +1,29 @@ +name: Debian Rolling Source Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + jazzy_debian: + name: Rolling debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml new file mode 100644 index 0000000000..d9ec610bbc --- /dev/null +++ b/.github/workflows/jazzy-pre-commit.yml @@ -0,0 +1,13 @@ +name: Pre-Commit - Jazzy + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..94ddfeb54f --- /dev/null +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -0,0 +1,28 @@ +name: RHEL Jazzy Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + jazzy_rhel: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml new file mode 100644 index 0000000000..7c5bdfc4dc --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -0,0 +1,47 @@ +name: Jazzy Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: jazzy + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml new file mode 100644 index 0000000000..f77e9a5060 --- /dev/null +++ b/.github/workflows/jazzy-source-build.yml @@ -0,0 +1,27 @@ +name: Jazzy Source Build +on: + workflow_dispatch: + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + with: + ros_distro: jazzy + ref: master + ros2_repo_branch: jazzy + container: ubuntu:24.04 diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 73055ef3e5..da3337554c 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' jobs: abi_check: diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index d0be23f076..529c13ac64 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -9,9 +9,26 @@ on: - master - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/rolling-check-docs.yml similarity index 67% rename from .github/workflows/ci-check-docs.yml rename to .github/workflows/rolling-check-docs.yml index 629279615b..a77a645e57 100644 --- a/.github/workflows/ci-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -1,8 +1,14 @@ -name: Check Docs +name: Rolling Check Docs on: workflow_dispatch: pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' jobs: check-docs: diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 8374afe8dc..3b4b195ff7 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -4,9 +4,29 @@ on: push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' jobs: coverage_rolling: @@ -14,4 +34,3 @@ jobs: secrets: inherit with: ros_distro: rolling - container: ubuntu:24.04 diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index cecd67603f..c1313d7d5d 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 4d91d19100..7bc07ba802 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -11,4 +11,3 @@ jobs: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master with: ros_distro: rolling - container: ubuntu:24.04 diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index a175d951b4..6b7fb6b8c4 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4784654db8..db0cd33de5 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -8,9 +8,27 @@ on: - master - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index ae38197455..66e53d1da1 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -4,6 +4,15 @@ on: push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml index 8b9545dff1..6bedaa0c97 100644 --- a/.github/workflows/update-pre-commit.yml +++ b/.github/workflows/update-pre-commit.yml @@ -5,7 +5,7 @@ name: Auto Update pre-commit on: workflow_dispatch: schedule: - - cron: '0 0 * * 0' # Run every Sunday at midnight + - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month jobs: auto_update_and_create_pr: diff --git a/README.md b/README.md index 7223ceae85..57a9ecdda1 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,8 @@ 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.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) **Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) diff --git a/ros2_controllers-not-released.jazzy.repos b/ros2_controllers-not-released.jazzy.repos new file mode 100644 index 0000000000..66352f4960 --- /dev/null +++ b/ros2_controllers-not-released.jazzy.repos @@ -0,0 +1,5 @@ +repositories: + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers.jazzy.repos b/ros2_controllers.jazzy.repos new file mode 100644 index 0000000000..8c20eccc96 --- /dev/null +++ b/ros2_controllers.jazzy.repos @@ -0,0 +1,25 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + 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 + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From 009f94686db6b6db845e81e3054b328988d163f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 8 May 2024 19:31:41 +0200 Subject: [PATCH 486/524] Deprecate non-stamped twist for tricycle_controller and steering_controllers (#1093) --- diff_drive_controller/doc/userdoc.rst | 2 +- steering_controllers_library/doc/userdoc.rst | 5 +---- .../src/steering_controllers_library.cpp | 3 +++ .../src/steering_controllers_library.yaml | 2 +- tricycle_controller/doc/userdoc.rst | 18 ++++++++++-------- .../src/tricycle_controller.cpp | 3 +++ .../src/tricycle_controller_parameter.yaml | 2 +- .../test/config/test_tricycle_controller.yaml | 1 - 8 files changed, 20 insertions(+), 16 deletions(-) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 70d0d7ca5c..897a7b3d9c 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -46,7 +46,7 @@ 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. + Velocity command for the controller. 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/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index df3d1529d0..0155bc5fd8 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -13,7 +13,7 @@ Nomenclature used for the controller is used from `wikipedia /reference [geometry_msgs/msg/TwistStamped] - If parameter ``use_stamped_vel`` is ``true``. -- /reference_unstamped [geometry_msgs/msg/Twist] - If parameter ``use_stamped_vel`` is ``false``. Publishers ,,,,,,,,,,, diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index eb497ebb3d..9bf9fa51d6 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -116,6 +116,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); if (params_.use_stamped_vel) { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index a9f7fa75fb..b18cac5ae1 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -116,7 +116,7 @@ steering_controllers_library: use_stamped_vel: { type: bool, default_value: false, - description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if + description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", read_only: false, } diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 991627eca2..30973762fd 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,14 +10,6 @@ Controller for mobile robots with a single double-actuated wheel, including trac Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. -Velocity commands ------------------ - -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. - - Other features -------------- @@ -26,6 +18,16 @@ Other features Velocity, acceleration and jerk limits Automatic stop after command timeout +ROS 2 Interfaces +------------------------ + +Subscribers +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + + Parameters -------------- diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 07be8c2aae..5586b87efc 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -316,6 +316,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } else { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index 68fc2202c2..951c159510 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -81,7 +81,7 @@ tricycle_controller: use_stamped_vel: { type: bool, default_value: true, - description: "Use stamp from input velocity message to calculate how old the command actually is.", + description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.", } traction: # "The positive limit will be applied to both directions. Setting different limits for positive " diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index efecfe968f..4cef3ee131 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -5,7 +5,6 @@ test_tricycle_controller: steering_joint_name: steering_joint wheel_radius: 0.125 wheelbase: 1.252 - use_stamped_vel: false enable_odom_tf: false use_sim_time: false pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] From 7170328194d40b823cd1972f26f7fc774a783897 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 9 May 2024 11:07:21 +0200 Subject: [PATCH 487/524] Add parameter check for geometric values (#1120) --- .../src/ackermann_steering_controller.yaml | 15 +++++++++++++++ .../src/bicycle_steering_controller.yaml | 9 +++++++++ .../src/diff_drive_controller_parameter.yaml | 6 ++++++ .../test/test_diff_drive_controller.cpp | 4 ++++ .../src/tricycle_controller_parameter.yaml | 6 ++++++ .../test/test_tricycle_controller.cpp | 3 +++ .../src/tricycle_steering_controller.yaml | 12 ++++++++++++ 7 files changed, 55 insertions(+) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 3726146919..1ec0b41c9f 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -5,6 +5,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheel_track: @@ -13,6 +16,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } wheelbase: @@ -21,6 +27,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheels_radius: @@ -29,6 +38,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Front wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheels_radius: @@ -37,4 +49,7 @@ ackermann_steering_controller: default_value: 0.0, description: "Rear wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index c40e27ef96..fde323ef74 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -5,6 +5,9 @@ bicycle_steering_controller: default_value: 0.0, description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheel_radius: @@ -13,6 +16,9 @@ bicycle_steering_controller: default_value: 0.0, description: "Front wheel radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheel_radius: @@ -21,4 +27,7 @@ bicycle_steering_controller: default_value: 0.0, description: "Rear wheel radius.", read_only: false, + validation: { + gt<>: [0.0] + } } diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 9720e068e1..755259cc2a 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -19,11 +19,17 @@ diff_drive_controller: type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } wheel_separation_multiplier: { type: double, diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 9ab3022a9f..f3f99ac8d8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -184,6 +184,10 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); parameter_overrides.push_back( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + // default parameters + parameter_overrides.push_back( + rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index 951c159510..b6d1fdf964 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -19,11 +19,17 @@ tricycle_controller: type: double, default_value: 0.0, description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } odom_frame_id: { type: string, diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5280aaf244..4be1680980 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -164,6 +164,9 @@ class TestTricycleController : public ::testing::Test rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); parameter_overrides.push_back( rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + // default parameters + parameter_overrides.push_back(rclcpp::Parameter("wheelbase", rclcpp::ParameterValue(1.))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 1015865fd9..6e5ae2b477 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -5,6 +5,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } wheelbase: @@ -13,6 +16,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheels_radius: @@ -21,6 +27,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Front wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheels_radius: @@ -29,4 +38,7 @@ tricycle_steering_controller: default_value: 0.0, description: "Rear wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } From b2a6b9b557fbc410816964cde363c5afb49c3013 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 9 May 2024 13:17:57 +0200 Subject: [PATCH 488/524] Remove non-existing parameter (#1119) --- .../test/config/test_diff_drive_controller.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index bfbf8f2d19..606bacbf4f 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -4,7 +4,6 @@ test_diff_drive_controller: right_wheel_names: ["right_wheels"] wheel_separation: 0.40 - wheels_per_side: 1 # actually 2, but both are controlled by 1 signal wheel_radius: 0.02 wheel_separation_multiplier: 1.0 From 30d52bd6cc9d439b471fcb31490fdff87c7a7550 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 13 May 2024 17:42:27 +0200 Subject: [PATCH 489/524] Fix the link for nav2 (#1132) --- doc/controllers_index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d042fe79dd..6f31fbee7a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -6,7 +6,7 @@ ros2_controllers ################# -Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. `Link to GitHub Repository `_ From cce280bc44b53b738efc3099632ccddf5e7024ea Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 14 May 2024 07:11:00 +0100 Subject: [PATCH 490/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 7 +++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 88 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index f14bd8ba01..fcd6e4b8cb 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 7dd8e0b321..4dd79a2418 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ * Use CMake target for eigen (`#1058 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index de67c5bc72..87e9ee6268 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index ae38961d10..35c5700d63 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove non-existing parameter (`#1119 `_) +* Add parameter check for geometric values (`#1120 `_) +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 306cc24ff6..c2cd6b94c0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index f4cbbe1bec..bdcc844fa7 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 607ec25f23..8b61bf5122 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bb4d699bd0..6a066e815c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ * Fix usage of visibility macros (`#1039 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6a9df05038..b4baa989b4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9762bcb55c..0678cffad1 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index d008380af9..195865d6b7 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Remove unused test code (`#1095 `_) +* Contributors: Bence Magyar + 4.7.0 (2024-03-22) ------------------ * Remove action_msg dependency (`#1077 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 82e631a75c..4633bc6bab 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [PID] Add example yaml to docs (`#951 `_) +* Contributors: Christoph Fröhlich + 4.7.0 (2024-03-22) ------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index cdb4165691..4a10fecdcb 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 89365b7754..dde17152a4 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2d7e5f08de..84d425f812 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a77c2635e8..f8cad522d1 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 416c399cc6..a8d9eeb31c 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ * [CI] Code coverage + pre-commit (`#1057 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 43e3cc30da..db6adbe1fd 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 150a575260..84b67554ca 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add parameter check for geometric values (`#1120 `_) +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * added conditioning to have rolling tags compilable in older versions (`#1071 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 04e3bf43fe..319a7dc6e1 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.7.0 (2024-03-22) ------------------ * Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e15ecc94e7..d144216d64 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.7.0 (2024-03-22) ------------------ From 6f1d71945d4039dc1f44294dd7405ec3970b50bd Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 14 May 2024 07:12:37 +0100 Subject: [PATCH 491/524] 4.8.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index fcd6e4b8cb..371af97240 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Add parameter check for geometric values (`#1120 `_) * add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 771494e67f..edd040ad5a 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.7.0 + 4.8.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 4dd79a2418..cc2d01d333 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index e9779eda4d..ae1ed5fc79 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.7.0 + 4.8.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 87e9ee6268..6715b36e3c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Add parameter check for geometric values (`#1120 `_) * add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 97c12290df..be87ada566 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.7.0 + 4.8.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 35c5700d63..c045692fc9 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Remove non-existing parameter (`#1119 `_) * Add parameter check for geometric values (`#1120 `_) * Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 5252803de9..c6bdc4e229 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.7.0 + 4.8.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 c2cd6b94c0..f69b29af93 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 9acfdd2ddc..92458dc25e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.7.0 + 4.8.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 bdcc844fa7..7b9b15f785 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index dee37dd565..f76fa39cea 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.7.0 + 4.8.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 8b61bf5122..9d2ed3f105 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1aed0d3c83..a15e03260b 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.7.0 + 4.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6a066e815c..c191f02239 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 7183bc7ebd..a942ef7362 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.7.0 + 4.8.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b4baa989b4..f453510963 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 3817b7a536..4bb3b025cb 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.7.0 + 4.8.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 0678cffad1..14c9594643 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 14f226450e..37e7ef518c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.7.0 + 4.8.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 195865d6b7..bb784700f4 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * [JTC] Remove unused test code (`#1095 `_) * Contributors: Bence Magyar diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index a44d55a731..fb1f180a40 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.7.0 + 4.8.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 4633bc6bab..6c45be4315 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * [PID] Add example yaml to docs (`#951 `_) * Contributors: Christoph Fröhlich diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 151862dc11..8fe803626e 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.7.0 + 4.8.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 4a10fecdcb..26975a6fee 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b56e3e154a..dc3ec1d315 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.7.0 + 4.8.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 dde17152a4..b396198081 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 4cdae222a5..1cf6c7600b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.7.0 + 4.8.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 84d425f812..399bbf198a 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index fc2e40fb8a..3051fd9f24 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.7.0 + 4.8.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 f8cad522d1..e0b2b19a05 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index fef330a0c7..45fd5f713c 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.7.0 + 4.8.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 273957f688..f085c09134 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.7.0", + version="4.8.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 a8d9eeb31c..bf3091b934 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c7ec8d4666..70bf579033 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.7.0 + 4.8.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 0339f47b5a..12eb399a61 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.7.0", + version="4.8.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 db6adbe1fd..f40ad793cf 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) * add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 97d03dc809..d7f1159cee 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.7.0 + 4.8.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 84b67554ca..ce997f2f8d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Add parameter check for geometric values (`#1120 `_) * Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) * add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 900f906a11..0bb5a61344 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.7.0 + 4.8.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 319a7dc6e1..2ac25f4c33 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ * Add parameter check for geometric values (`#1120 `_) * add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index ccb973be40..a0c90d80a2 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.7.0 + 4.8.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 d144216d64..71946c0e55 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.8.0 (2024-05-14) +------------------ 4.7.0 (2024-03-22) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index a6c28a3cd1..2c37e0aec6 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.7.0 + 4.8.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 4008030db301c48d3a03f83434f48800981fbf9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 14 May 2024 08:49:50 +0200 Subject: [PATCH 492/524] [CI] Update branch of reusable workflow (#1135) * Update branch of reusable workflow * Include workflow file to paths filter --- .github/workflows/rolling-check-docs.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index a77a645e57..15b7eff57e 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -9,10 +9,11 @@ on: - '**.rst' - '**.md' - '**.yaml' + - '.github/workflows/rolling-check-docs.yml' jobs: check-docs: name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling with: ROS2_CONTROLLERS_PR: ${{ github.ref }} From b51e4c2436814124bd30a215d949c6062ceb1237 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 22 May 2024 11:18:34 -0600 Subject: [PATCH 493/524] JTC trajectory end time validation fix (#1090) --- .../src/joint_trajectory_controller.cpp | 19 ++++++++----------- .../test/test_trajectory_controller.cpp | 11 +++++++++++ 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a4b63cb75..b250b25236 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1370,17 +1370,20 @@ bool JointTrajectoryController::validate_trajectory_msg( return false; } + if (trajectory.points.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; + } + const auto trajectory_start_time = static_cast(trajectory.header.stamp); // If the starting time it set to 0.0, it means the controller should start it now. // Otherwise we check if the trajectory ends before the current time, // in which case it can be ignored. if (trajectory_start_time.seconds() != 0.0) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } + auto const trajectory_end_time = + trajectory_start_time + trajectory.points.back().time_from_start; if (trajectory_end_time < get_node()->now()) { RCLCPP_ERROR( @@ -1405,12 +1408,6 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - if (trajectory.points.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); - return false; - } - if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b00bd5380..611c1d4c1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1384,6 +1384,17 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg = good_traj_msg; traj_msg.points.push_back(traj_msg.points.front()); EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the past + traj_msg = good_traj_msg; + traj_msg.header.stamp = rclcpp::Time(1); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the future + traj_msg = good_traj_msg; + traj_msg.header.stamp = traj_controller_->get_node()->now(); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(10); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); } /** From 731da8e43493f9da7e1be2eb8cf13154b1b240a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 22 May 2024 19:23:23 +0200 Subject: [PATCH 494/524] Add custom rosdoc2 config for ros2_controllers metapackage (#1100) * Add simple rosdoc2 landing page * Add link to package.xml * Add links to controller docs on control.ros.org * Set check-yaml syntax instead of ignoring --- .github/workflows/rosdoc2.yml | 14 ++++++ .pre-commit-config.yaml | 2 + ros2_controllers/doc/conf.py | 5 +++ ros2_controllers/doc/index.rst | 80 ++++++++++++++++++++++++++++++++++ ros2_controllers/package.xml | 2 + ros2_controllers/rosdoc2.yaml | 20 +++++++++ 6 files changed, 123 insertions(+) create mode 100644 .github/workflows/rosdoc2.yml create mode 100644 ros2_controllers/doc/conf.py create mode 100644 ros2_controllers/doc/index.rst create mode 100644 ros2_controllers/rosdoc2.yaml diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml new file mode 100644 index 0000000000..0771248e79 --- /dev/null +++ b/.github/workflows/rosdoc2.yml @@ -0,0 +1,14 @@ +name: rosdoc2 + +on: + workflow_dispatch: + pull_request: + paths: + - ros2_controllers/doc/** + - ros2_controllers/rosdoc2.yaml + - ros2_controllers/package.xml + + +jobs: + check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a34629da31..de19328805 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -26,6 +26,7 @@ repos: - id: check-symlinks - id: check-xml - id: check-yaml + args: ["--allow-multiple-documents"] - id: debug-statements - id: end-of-file-fixer - id: mixed-line-ending @@ -104,6 +105,7 @@ repos: description: Check if copyright notice is available in all files. entry: ament_copyright language: system + exclude: .*/conf\.py$ # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/ros2_controllers/doc/conf.py b/ros2_controllers/doc/conf.py new file mode 100644 index 0000000000..b6134e9abd --- /dev/null +++ b/ros2_controllers/doc/conf.py @@ -0,0 +1,5 @@ +# Configuration file for the Sphinx documentation builder. +# settings will be overridden by rosdoc2, so we add here only custom settings + +copyright = "2024, ros2_control development team" +html_logo = "https://control.ros.org/master/_static/logo_ros-controls.png" diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst new file mode 100644 index 0000000000..f785112944 --- /dev/null +++ b/ros2_controllers/doc/index.rst @@ -0,0 +1,80 @@ +Welcome to the documentation for ros2_controllers +================================================= + +For more information of the ros2_control framework see `control.ros.org `__. + +.. list-table:: + :header-rows: 1 + + * - Package Name + - Documentation + - API + - ROS Index + * - ackermann_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - admittance_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - bicycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - diff_drive_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - effort_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - force_torque_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - forward_command_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - imu_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_state_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_trajectory_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - pid_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - position_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - range_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - steering_controllers_library + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - velocity_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3051fd9f24..e8d47a5e48 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -8,6 +8,8 @@ Apache License 2.0 + https://control.ros.org + ament_cmake ackermann_steering_controller diff --git a/ros2_controllers/rosdoc2.yaml b/ros2_controllers/rosdoc2.yaml new file mode 100644 index 0000000000..c5ec1b53a0 --- /dev/null +++ b/ros2_controllers/rosdoc2.yaml @@ -0,0 +1,20 @@ +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + # Not generating any documentation of code + generate_package_index: false + always_run_doxygen: false + enable_breathe: false + enable_exhale: false + always_run_sphinx_apidoc: false + +builders: + # Configure Sphinx with the location of the docs: + - sphinx: { + name: 'ros2_controllers', + sphinx_sourcedir: 'doc', + output_dir: '' + } From d2e926be5d1ecc31fc5966322c59afdd17bb1c3f Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 27 May 2024 18:50:45 +0200 Subject: [PATCH 495/524] Fix correct usage of angular velocity in update_odometry() function (#1118) --- .../src/steering_odometry.cpp | 4 +-- .../test/test_steering_odometry.cpp | 36 +++++++++++++++++++ 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index aadd047f2e..6fb20478a4 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -52,7 +52,7 @@ bool SteeringOdometry::update_odometry( const double linear_velocity, const double angular, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +62,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular / dt); + angular_acc_.accumulate(angular); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 173c76baef..3e4adc59fe 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -108,3 +108,39 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) EXPECT_LT(cmd1[0], 0); } + +TEST(TestSteeringOdometry, bicycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, tricycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} From 86987196906db9ef3d23c0e9ca0b097e735da586 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 1 Jun 2024 13:21:14 +0200 Subject: [PATCH 496/524] Bump version of pre-commit hooks (#1157) * Bump version of pre-commit hooks * Fix codespell --------- Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> Co-authored-by: Christoph Froehlich --- .pre-commit-config.yaml | 6 +++--- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- tricycle_controller/src/tricycle_controller.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index de19328805..c75acbd76d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.4 + rev: v18.1.5 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -126,14 +126,14 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.2 + rev: 0.28.4 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 42b6cda8e1..a64f210503 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -352,7 +352,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( received_velocity_msg_ptr_.set(std::move(msg)); }); - // initialize odometry publisher and messasge + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 5586b87efc..66c6ed1b0f 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -338,7 +338,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & }); } - // initialize odometry publisher and messasge + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = From 0ae3e42d779daaea49a76803c839241aba447829 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 5 Jun 2024 19:10:50 +0200 Subject: [PATCH 497/524] [RQT-JTC] limits from jtc controlled joints (#1146) --- .../joint_limits_urdf.py | 94 ++++++++++--------- .../joint_trajectory_controller.py | 3 +- 2 files changed, 50 insertions(+), 47 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 5655e12f7a..bdde80dd54 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) -def get_joint_limits(node, use_smallest_joint_limits=True): +def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True): if jtype == "fixed": continue name = child.getAttribute("name") - try: - limit = child.getElementsByTagName("limit")[0] + + if name in joints_names: try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - minval = -pi - maxval = pi - else: + limit = child.getElementsByTagName("limit")[0] + try: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: raise Exception( - f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" ) - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: + except IndexError: raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing limits tag for the joint : {name} in the robot_description!" ) - except IndexError: - raise Exception( - f"Missing limits tag for the joint : {name} in the robot_description!" - ) - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - if name in dependent_joints: - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + safety_tags = child.getElementsByTagName("safety_controller") + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) + + dependent_joints[name] = entry + continue + + if name in dependent_joints: + continue + + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel + free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 72c11625e4..4cc6c901af 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -238,7 +238,8 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + for jtc_info in running_jtc: + self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info)) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: From d076ea2fe0555e8dc44cea1e3f9b4fc261ec4d1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 19:39:17 +0200 Subject: [PATCH 498/524] Add mobile robot kinematics 101 and improve steering library docs (#954) --- diff_drive_controller/doc/userdoc.rst | 2 + doc/controllers_index.rst | 7 +- doc/images/.gitignore | 1 + doc/images/ackermann_steering.drawio | 251 ++++++++++++++ doc/images/ackermann_steering.svg | 3 + doc/images/ackermann_steering_traction.drawio | 287 ++++++++++++++++ doc/images/ackermann_steering_traction.svg | 3 + doc/images/car_like_robot.drawio | 173 ++++++++++ doc/images/car_like_robot.svg | 3 + doc/images/diff_drive.drawio | 137 ++++++++ doc/images/diff_drive.svg | 3 + doc/images/double_traction.drawio | 197 +++++++++++ doc/images/double_traction.svg | 3 + doc/images/unicycle.drawio | 89 +++++ doc/images/unicycle.svg | 3 + doc/mobile_robot_kinematics.rst | 322 ++++++++++++++++++ steering_controllers_library/doc/userdoc.rst | 79 +++-- tricycle_controller/doc/userdoc.rst | 3 + 18 files changed, 1533 insertions(+), 33 deletions(-) create mode 100644 doc/images/.gitignore create mode 100644 doc/images/ackermann_steering.drawio create mode 100644 doc/images/ackermann_steering.svg create mode 100644 doc/images/ackermann_steering_traction.drawio create mode 100644 doc/images/ackermann_steering_traction.svg create mode 100644 doc/images/car_like_robot.drawio create mode 100644 doc/images/car_like_robot.svg create mode 100644 doc/images/diff_drive.drawio create mode 100644 doc/images/diff_drive.svg create mode 100644 doc/images/double_traction.drawio create mode 100644 doc/images/double_traction.svg create mode 100644 doc/images/unicycle.drawio create mode 100644 doc/images/unicycle.svg create mode 100644 doc/mobile_robot_kinematics.rst diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 897a7b3d9c..29df4e55ba 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -11,6 +11,8 @@ As input it takes velocity commands for the robot body, which are translated to Odometry is computed from hardware feedback and published. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + Other features -------------- diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 6f31fbee7a..2cb55150ce 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -21,18 +21,15 @@ Guidelines and Best Practices * -Controllers for Mobile Robots -***************************** +Controllers for Wheeled Mobile Robots +************************************* .. toctree:: :titlesonly: - Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Controllers for Manipulators and Other Robots ********************************************* diff --git a/doc/images/.gitignore b/doc/images/.gitignore new file mode 100644 index 0000000000..8d71bf9cf4 --- /dev/null +++ b/doc/images/.gitignore @@ -0,0 +1 @@ +*.bkp diff --git a/doc/images/ackermann_steering.drawio b/doc/images/ackermann_steering.drawio new file mode 100644 index 0000000000..d0ddb06a56 --- /dev/null +++ b/doc/images/ackermann_steering.drawio @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering.svg b/doc/images/ackermann_steering.svg new file mode 100644 index 0000000000..1ca6ace166 --- /dev/null +++ b/doc/images/ackermann_steering.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/ackermann_steering_traction.drawio b/doc/images/ackermann_steering_traction.drawio new file mode 100644 index 0000000000..ccf40be4d7 --- /dev/null +++ b/doc/images/ackermann_steering_traction.drawio @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering_traction.svg b/doc/images/ackermann_steering_traction.svg new file mode 100644 index 0000000000..1ee9d0181b --- /dev/null +++ b/doc/images/ackermann_steering_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/car_like_robot.drawio b/doc/images/car_like_robot.drawio new file mode 100644 index 0000000000..f9b87b9794 --- /dev/null +++ b/doc/images/car_like_robot.drawio @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/car_like_robot.svg b/doc/images/car_like_robot.svg new file mode 100644 index 0000000000..6fb6bf17d0 --- /dev/null +++ b/doc/images/car_like_robot.svg @@ -0,0 +1,3 @@ + + +
ICR
Front wheel
Rear wheel
 diff --git a/doc/images/diff_drive.drawio b/doc/images/diff_drive.drawio new file mode 100644 index 0000000000..f5fc9802e0 --- /dev/null +++ b/doc/images/diff_drive.drawio @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/diff_drive.svg b/doc/images/diff_drive.svg new file mode 100644 index 0000000000..9759b86048 --- /dev/null +++ b/doc/images/diff_drive.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/images/double_traction.drawio b/doc/images/double_traction.drawio new file mode 100644 index 0000000000..7f9989d733 --- /dev/null +++ b/doc/images/double_traction.drawio @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/double_traction.svg b/doc/images/double_traction.svg new file mode 100644 index 0000000000..ec60b11ffa --- /dev/null +++ b/doc/images/double_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/unicycle.drawio b/doc/images/unicycle.drawio new file mode 100644 index 0000000000..49a1f44b5a --- /dev/null +++ b/doc/images/unicycle.drawio @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/unicycle.svg b/doc/images/unicycle.svg new file mode 100644 index 0000000000..826bbb1463 --- /dev/null +++ b/doc/images/unicycle.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst new file mode 100644 index 0000000000..2ab4f1c0ee --- /dev/null +++ b/doc/mobile_robot_kinematics.rst @@ -0,0 +1,322 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst + +.. _mobile_robot_kinematics: + +Wheeled Mobile Robot Kinematics +-------------------------------------------------------------- + +.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 +.. _modern_robotics: http://modernrobotics.org/ + +This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. + +Wheeled mobile robots can be classified in two categories: + +Omnidirectional robots + which can move instantaneously in any direction in the plane, and + +Nonholonomic robots + which cannot move instantaneously in any direction in the plane. + +The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. + +Omnidirectional Wheeled Mobile Robots +..................................... + +Robots with omniwheels or mecanum wheels. Section will be updated if controllers for these robots are implemented. + +Nonholonomic Wheeled Mobile Robots +..................................... + +Unicycle model +,,,,,,,,,,,,,,,, + +To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model + +.. image:: images/unicycle.svg + :width: 550 + :align: center + :alt: Unicycle + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. +* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. + +In the following, we want to command the robot with a desired body twist + +.. math:: + + \vec{\nu}_b = \begin{bmatrix} + \vec{\omega}_{b} \\ + \vec{v}_{b} + \end{bmatrix}, + +where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give + +* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. +* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. + +as desired system inputs. The forward kinematics of the unicycle can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \omega_{b,z} + +We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. + +Differential Drive Robot +,,,,,,,,,,,,,,,,,,,,,,,, + +Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: + +.. code-block:: text + + A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) + is a robot with a serious problem of balance in static conditions. However, + there exist vehicles that are kinematically equivalent to a unicycle but more + stable from a mechanical viewpoint. + +One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. + +.. image:: images/diff_drive.svg + :width: 550 + :align: center + :alt: Differential drive robot + +* :math:`w` is the wheel track (the distance between the wheels). + +**Forward Kinematics** + +The forward kinematics of the differential drive model can be calculated from the unicycle model above using + +.. math:: + v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ + \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} + +**Inverse Kinematics** + +The necessary wheel speeds to achieve a desired body twist can be calculated with: + +.. math:: + + v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ + v_{right} &= v_{b,x} + \omega_{b,z} w / 2 + + +**Odometry** + +We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. + +Car-Like (Bicycle) Model +,,,,,,,,,,,,,,,,,,,,,,,, + +The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. + +.. image:: images/car_like_robot.svg + :width: 550 + :align: center + :alt: Car-like robot + +* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. +* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. +* :math:`l` is the wheelbase. + +We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. + +As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: + +.. math:: + v_{rear} = v_{front} \cos(\phi) + +**Forward Kinematics** + +The forward kinematics of the car-like model can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) + + +**Inverse Kinematics** + +The steering angle is one command input of the robot: + +.. math:: + \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) + + +For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: + +.. math:: + v_{rear} = v_{b,x} + + +For the front-wheel drive, the velocity of the front wheel is the second input of the robot: + +.. math:: + v_{front} = \frac{v_{b,x}}{\cos(\phi)} + +**Odometry** + +We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. + +For the rear wheel case: + +.. math:: + \dot{x} &= v_{rear} \cos(\theta) \\ + \dot{y} &= v_{rear} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) + + +For the front wheel case: + +.. math:: + \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ + \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ + \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) + + +Double-Traction Axle +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. + +.. image:: images/double_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two traction wheels at the rear + +* :math:`w_r` is the wheel track of the rear axle. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the velocity of the rear wheels must satisfy these conditions to avoid skidding + +.. math:: + v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ + v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). + + +Ackermann Steering +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled robot with two independent steering wheels in the front. + +.. image:: images/ackermann_steering.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front + +* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. + +To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. +This is the so-called **Ackermann steering**. + +.. note:: + Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. + +**Forward Kinematics** + +The forward kinematics is the same as for the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the steering angles of the front wheels must satisfy these conditions to avoid skidding + +.. math:: + \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ + \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) + +**Odometry** + +The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. +If there is no slip and the measurements are ideal, + +.. math:: + \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). + +Ackermann Steering with Traction +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. + +.. image:: images/ackermann_steering_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front, which are also independently driven. + +* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and + +.. math:: + \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} + +with turning radius of the robot and the left/right front wheel + +.. math:: + R_b &= \frac{l}{\tan(\phi)} \\ + R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ + R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. + +This results in the following inverse kinematics equations + +.. math:: + v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ + v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} + +with the steering angles of the front wheels from the Ackermann steering equations above. + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 0155bc5fd8..a52e34c120 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -5,17 +5,25 @@ steering_controllers_library ============================= -Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). +.. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg +.. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg +.. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg + +Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. + The library implements generic odometry and update methods and defines the main interfaces. -Nomenclature used for the controller is used from `wikipedia `_. +The update methods only use inverse kinematics, it does not implement any feedback control loops like path-tracking controllers etc. + +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped Twist messages where linear ``x`` and angular ``z`` components are used. -Angular component under +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. Values in other components are ignored. + In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: @@ -24,65 +32,80 @@ Other relevant features are: * input command timeout based on a parameter. The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. -Currently implemented kinematics in corresponding packages are: + +Currently implemented kinematics +-------------------------------------------------------------- * :ref:`Bicycle ` - with one steering and one drive joints; * :ref:`Tricylce ` - with one steering and two drive joints; -* :ref:`Ackermann ` - with two seering and two drive joints. +* :ref:`Ackermann ` - with two steering and two drive joints. +.. toctree:: + :hidden: + Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> + Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> Description of controller's interfaces -------------------------------------- References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -- /linear/velocity [double], in m/s -- /angular/position [double] # in [rad] -Commands -,,,,,,,,, -``front_steering == true`` +Used when controller is in chained mode (``in_chained_mode == true``). + +- ``/linear/velocity`` double, in m/s +- ``/angular/position`` double, in rad -- /position [double] # in [rad] -- /velocity [double] # in [m/s] +Command interfaces +,,,,,,,,,,,,,,,,,,, -``front_steering == false`` +If parameter ``front_steering == true`` -- /velocity [double] # in [m/s] -- /position [double] # in [rad] +- ``/position`` double, in rad +- ``/velocity`` double, in m/s + +If parameter ``front_steering == false`` + +- ``/velocity`` double, in m/s +- ``/position`` double, in rad + +State interfaces +,,,,,,,,,,,,,,,,, -States -,,,,,,, Depending on the ``position_feedback``, different feedback types are expected * ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` * ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` -``front_steering == true`` +If parameter ``front_steering == true`` -- /position [double] # in [rad] -- / [double] # in [m] or [m/s] +- ``/position`` double, in rad +- ``/`` double, in m or m/s -``front_steering == false`` +If parameter ``front_steering == false`` -- / [double] # [m] or [m/s] -- /position [double] # in [rad] +- ``/`` double, in m or m/s +- ``/position`` double, in rad Subscribers ,,,,,,,,,,,, + Used when controller is not in chained mode (``in_chained_mode == false``). -- /reference [geometry_msgs/msg/TwistStamped] +- ``/reference`` [`geometry_msgs/msg/TwistStamped `_] Publishers ,,,,,,,,,,, -- /odometry [nav_msgs/msg/Odometry] -- /tf_odometry [tf2_msgs/msg/TFMessage] -- /controller_state [control_msgs/msg/SteeringControllerStatus] + +- ``/odometry`` [`nav_msgs/msg/Odometry `_] +- ``/tf_odometry`` [`tf2_msgs/msg/TFMessage `_] +- ``/controller_state`` [`control_msgs/msg/SteeringControllerStatus `_] Parameters ,,,,,,,,,,, + This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 30973762fd..2ede52e1b6 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,6 +10,9 @@ Controller for mobile robots with a single double-actuated wheel, including trac Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + + Other features -------------- From 619e3036afa0bc305bc10926e91e9806026ef32f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Jun 2024 18:50:13 +0100 Subject: [PATCH 499/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 78 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 371af97240..0f0cb49cc9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc2d01d333..b6e0bc3481 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 6715b36e3c..879aeee9ae 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c045692fc9..d343ddd536 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + 4.8.0 (2024-05-14) ------------------ * Remove non-existing parameter (`#1119 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index f69b29af93..aa905fd2df 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7b9b15f785..8675e4135c 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 9d2ed3f105..719ac12c2d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c191f02239..246d1b9b1b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index f453510963..c3573441cf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 14c9594643..eeeb2a1663 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bb784700f4..df8de77c23 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* JTC trajectory end time validation fix (`#1090 `_) +* Contributors: Henry Moore + 4.8.0 (2024-05-14) ------------------ * [JTC] Remove unused test code (`#1095 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 6c45be4315..48f8796679 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * [PID] Add example yaml to docs (`#951 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 26975a6fee..7ffffa6270 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b396198081..cfb6952bd6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 399bbf198a..818c437909 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e0b2b19a05..4770c8a5b3 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index bf3091b934..586a28fa37 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RQT-JTC] limits from jtc controlled joints (`#1146 `_) +* Contributors: Jakub Delicat + 4.8.0 (2024-05-14) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index f40ad793cf..a7d1768f7b 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) +* Contributors: Christoph Fröhlich, Ferry Schoenmakers + 4.8.0 (2024-05-14) ------------------ * Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index ce997f2f8d..cca8c443be 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 2ac25f4c33..2528a7d6f9 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 71946c0e55..e1ed29b996 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ From 0b8a196b23538333bc4a92561214cc41b3e43530 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Jun 2024 18:50:53 +0100 Subject: [PATCH 500/524] 4.9.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 0f0cb49cc9..5cfdefb83b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index edd040ad5a..7b66dd765b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.8.0 + 4.9.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 b6e0bc3481..1a2dd8ed96 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index ae1ed5fc79..2add40baf7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.8.0 + 4.9.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 879aeee9ae..8c3d27e24f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index be87ada566..bcad405abe 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.8.0 + 4.9.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 d343ddd536..a4473f0f83 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Bump version of pre-commit hooks (`#1157 `_) * Contributors: Christoph Fröhlich, github-actions[bot] diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index c6bdc4e229..8765d2506f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.8.0 + 4.9.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 aa905fd2df..63b44cd218 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 92458dc25e..6519a07b07 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.8.0 + 4.9.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 8675e4135c..deca56a197 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f76fa39cea..4576bbb8b0 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.8.0 + 4.9.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 719ac12c2d..3f1917dcad 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index a15e03260b..b4f34fce8c 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.8.0 + 4.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 246d1b9b1b..e0b5a0a311 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a942ef7362..f4ee00591a 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.8.0 + 4.9.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c3573441cf..cebfc8d78c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 4bb3b025cb..032e5addc1 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.8.0 + 4.9.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 eeeb2a1663..ec9b79726f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 37e7ef518c..a34c05b269 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.8.0 + 4.9.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 df8de77c23..544cf500ef 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * JTC trajectory end time validation fix (`#1090 `_) * Contributors: Henry Moore diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fb1f180a40..2c04d7fa23 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.8.0 + 4.9.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 48f8796679..4b884dc386 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 8fe803626e..be598f94ae 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.8.0 + 4.9.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7ffffa6270..f454aadc4e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index dc3ec1d315..37bcdbe108 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.8.0 + 4.9.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 cfb6952bd6..7dd295970d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 1cf6c7600b..31e8b1755b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.8.0 + 4.9.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 818c437909..af872ec8a3 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e8d47a5e48..278ecb0b1a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.8.0 + 4.9.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 4770c8a5b3..19ead3093f 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 45fd5f713c..c321c9703d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.8.0 + 4.9.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 f085c09134..e0dfb4d65e 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.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 586a28fa37..121a5066c2 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * [RQT-JTC] limits from jtc controlled joints (`#1146 `_) * Contributors: Jakub Delicat diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 70bf579033..68a173217a 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.8.0 + 4.9.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 12eb399a61..e9a19034c6 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.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 a7d1768f7b..e1cdc069df 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) * Contributors: Christoph Fröhlich, Ferry Schoenmakers diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index d7f1159cee..08d252d794 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.8.0 + 4.9.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 cca8c443be..8f36bae23c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Bump version of pre-commit hooks (`#1157 `_) * Contributors: Christoph Fröhlich, github-actions[bot] diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0bb5a61344..51fe867106 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.8.0 + 4.9.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 2528a7d6f9..f6423146a0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index a0c90d80a2..6e1ed66edd 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.8.0 + 4.9.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 e1ed29b996..64a09e4309 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 2c37e0aec6..05f6245686 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.8.0 + 4.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 9625702643879d21468aec25fbb0a5dbec0f6b81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 20:14:35 +0200 Subject: [PATCH 501/524] Remove manual angle-wraparound parameter (#1152) --- doc/release_notes/Jazzy.rst | 2 +- .../src/joint_trajectory_controller.cpp | 11 ----------- .../src/joint_trajectory_controller_parameters.yaml | 6 ------ 3 files changed, 1 insertion(+), 18 deletions(-) diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 8ecac3508f..4c5db18f3e 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -32,7 +32,7 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). -* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. pid_controller ************************ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b250b25236..b80d1b7908 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -703,17 +703,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - if (gains.angle_wraparound) - { - // TODO(christophfroehlich): remove this warning in a future release (ROS-J) - RCLCPP_WARN( - logger, - "[Deprecated] Parameter 'gains..angle_wraparound' is deprecated. The " - "angle_wraparound is now used if a continuous joint is configured in the URDF."); - joints_angle_wraparound_[i] = true; - } - if (!urdf_.empty()) { auto urdf_joint = model_.getJoint(params_.joints[i]); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b17229cab8..8bd64b6314 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -122,12 +122,6 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling :math:`k_{ff}` of velocity" } - angle_wraparound: { - type: bool, - default_value: false, - description: "[deprecated] For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." - } constraints: stopped_velocity_tolerance: { type: double, From 64adf67017860bf414f27153efdecfca568d7c87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 20:14:54 +0200 Subject: [PATCH 502/524] Remove unstamped twist subscribers + parameters (#1151) --- .../ackermann_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test_ackermann_steering_controller.hpp | 1 - .../bicycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test/test_bicycle_steering_controller.hpp | 1 - .../steering_controllers_library.hpp | 2 - .../src/steering_controllers_library.cpp | 55 +--------------- .../src/steering_controllers_library.yaml | 8 --- .../steering_controllers_library_params.yaml | 1 - .../test_steering_controllers_library.hpp | 1 - .../config/tricycle_drive_controller.yaml | 1 - .../tricycle_controller.hpp | 2 - .../src/tricycle_controller.cpp | 63 +++++-------------- .../src/tricycle_controller_parameter.yaml | 5 -- .../test_tricycle_steering_controller.hpp | 1 - .../tricycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - 18 files changed, 20 insertions(+), 127 deletions(-) diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6b64f901c3..6064814d32 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index ecb1b071e3..5d42adcdd5 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a047186d14..49331ae8a2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -280,7 +280,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index a2a6c6508b..3a656cc724 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml index 39ffeed878..e1b9c1ab72 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_wheel_joint] diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 5e21ff228c..390447c25f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -251,7 +251,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..c11d90dc6f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -98,7 +98,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms @@ -143,7 +142,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); - void reference_callback_unstamped(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 9bf9fa51d6..5880000d17 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -114,22 +114,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - } - else - { - ref_subscriber_unstamped_ = get_node()->create_subscription( - "~/reference_unstamped", subscribers_qos, - std::bind( - &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); - } + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -244,42 +231,6 @@ void SteeringControllersLibrary::reference_callback( } } -void SteeringControllersLibrary::reference_callback_unstamped( - const std::shared_ptr msg) -{ - RCLCPP_WARN( - get_node()->get_logger(), - "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " - "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " - "future."); - auto twist_stamped = *(input_ref_.readFromNonRT()); - twist_stamped->header.stamp = get_node()->now(); - // if no timestamp provided use current time for command timestamp - if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - twist_stamped->header.stamp = get_node()->now(); - } - - const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - twist_stamped->twist = *msg; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - controller_interface::InterfaceConfiguration SteeringControllersLibrary::command_interface_configuration() const { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index b18cac5ae1..711a780458 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -112,11 +112,3 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } - - use_stamped_vel: { - type: bool, - default_value: false, - description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if - use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", - read_only: false, - } diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index d200d34961..01bfb02302 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,7 +6,6 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1284b72096..05ac17b454 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -301,7 +301,6 @@ class SteeringControllersLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index fba6f34bb2..a7853954d7 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -36,7 +36,6 @@ tricycle_controller: # cmd_vel input cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received - use_stamped_vel: false # Set to True if using TwistStamped. # Debug publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 24aaf89de9..b6f9aae417 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -140,8 +140,6 @@ class TricycleController : 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}; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 66c6ed1b0f..94ff63e659 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -239,7 +239,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command = get_node()->get_parameter("publish_ackermann_command").as_bool(); - params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { @@ -291,52 +290,25 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (params_.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 - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + 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)) { - 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_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 message odometry_publisher_ = get_node()->create_publisher( @@ -460,7 +432,6 @@ bool TricycleController::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/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index b6d1fdf964..cf661eddc1 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -84,11 +84,6 @@ tricycle_controller: gt<>: [0] } } - use_stamped_vel: { - type: bool, - default_value: true, - description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.", - } traction: # "The positive limit will be applied to both directions. Setting different limits for positive " # "and negative directions is not supported. Actuators are " diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6a516691b8..1807ab59a8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -265,7 +265,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 6bfb87a892..cc640e1503 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index ea8b88002e..7cb2e4906f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] From b24515538cd31b6c2b64268465dd479f464b151d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 21:50:09 +0200 Subject: [PATCH 503/524] Fix steering controllers library code documentation and naming (#1149) * Update documentation and consolidate variable names * Simplify private methods and further update docs * Rename methods * Rename method and variables * Rename convert method * Rename variables and improve doc * Rename local variables * Use std::isfinite instead of !isnan Co-authored-by: Sai Kishor Kothakota * Use a lowercase theta for heading Co-authored-by: Sai Kishor Kothakota * Make some temporary variables const * Let update_from_position call update_from_velocity * Explicitly set variables with 0 in constructor * Fix docstring * Apply consistent variable naming Co-authored-by: Quique Llorente --------- Co-authored-by: Sai Kishor Kothakota Co-authored-by: Quique Llorente --- .../src/ackermann_steering_controller.cpp | 23 +-- .../src/bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 63 +++---- .../src/steering_odometry.cpp | 175 ++++++++---------- .../src/tricycle_steering_controller.cpp | 18 +- 5 files changed, 139 insertions(+), 150 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c3a7539c5a..d9d95bf8b5 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double front_right_steer_position = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 5f013d7d7a..95eaf1965c 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); } } } 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 e4a22f6d3b..0104b011c7 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -61,7 +61,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -72,7 +72,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_pos Right traction wheel velocity [rad] * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -96,7 +96,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -107,7 +107,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -130,11 +130,11 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void update_open_loop(const double linear, const double angular, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt); /** * \brief Set odometry type @@ -175,22 +175,23 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + void set_wheel_params( + const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size */ - void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); /** * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double Vx, const double theta_dot); + const double v_bx, const double omega_bz); /** * \brief Reset poses, heading, and accumulators @@ -199,35 +200,35 @@ class SteeringOdometry private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update - * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) - * computed by previous odometry method \param angular Angular velocity [rad] (angular - * displacement, i.e. m/s * dt) computed by previous odometry method + * \brief Uses precomputed linear and angular velocities to compute odometry + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - bool update_odometry(const double linear_velocity, const double angular, const double dt); + bool update_odometry(const double v_bx, const double omega_bz, const double dt); /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_runge_kutta_2(double linear, double angular); + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \brief Integrates the velocities (linear and angular) + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_exact(double linear, double angular); + void integrate_fk(const double v_bx, const double omega_bz, const double dt); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param omega_bz Angular velocity of the robot around x_z-axis */ - double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); /** * \brief Reset linear and angular accumulators diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 6fb20478a4..32cdb69454 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) wheelbase_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), + traction_right_wheel_old_pos_(0.0), + traction_left_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size) @@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular, const double dt) + const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); + integrate_fk(linear_velocity, angular_velocity, dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular); + angular_acc_.accumulate(angular_velocity); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); @@ -73,70 +75,47 @@ bool SteeringOdometry::update_odometry( bool SteeringOdometry::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; - const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; + const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; /// Update old position with current: - traction_wheel_old_pos_ = traction_wheel_cur_pos; + traction_wheel_old_pos_ = traction_wheel_pos; - /// Compute linear and angular diff: - const double linear_velocity = traction_wheel_est_pos_diff / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - /// Compute linear and angular diff: - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos, + left_steer_pos, dt); } bool SteeringOdometry::update_from_velocity( @@ -144,9 +123,9 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -157,9 +136,9 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -169,19 +148,19 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + linear_ = v_bx; + angular_ = omega_bz; /// Integrate odometry: - SteeringOdometry::integrate_exact(linear * dt, angular * dt); + integrate_fk(v_bx, omega_bz, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) @@ -200,36 +179,37 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } -double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (theta_dot == 0 || Vx == 0) + if (omega_bz == 0 || v_bx == 0) { return 0; } - return std::atan(theta_dot * wheelbase_ / Vx); + return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double Vx, const double theta_dot) + const double v_bx, const double omega_bz) { - // desired velocity and steering angle of the middle of traction and steering axis - double Ws, alpha; + // desired wheel speed and steering angle of the middle of traction and steering axis + double Ws, phi; - if (Vx == 0 && theta_dot != 0) + if (v_bx == 0 && omega_bz != 0) { - alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); } if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; - std::vector steering_commands = {alpha}; + std::vector steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == TRICYCLE_CONFIG) @@ -242,12 +222,12 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } - steering_commands = {alpha}; + steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == ACKERMANN_CONFIG) @@ -257,21 +237,23 @@ std::tuple, std::vector> SteeringOdometry::get_comma if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; - steering_commands = {alpha, alpha}; + steering_commands = {phi, phi}; } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - double numerator = 2 * wheelbase_ * std::sin(alpha); - double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); - double denominator_second_member = wheel_track_ * std::sin(alpha); + const double numerator = 2 * wheelbase_ * std::sin(phi); + const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); + const double denominator_second_member = wheel_track_ * std::sin(phi); - double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); + const double alpha_r = + std::atan2(numerator, denominator_first_member + denominator_second_member); + const double alpha_l = + std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); @@ -290,35 +272,36 @@ void SteeringOdometry::reset_odometry() reset_accumulators(); } -void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +void SteeringOdometry::integrate_runge_kutta_2( + const double v_bx, const double omega_bz, const double dt) { - const double direction = heading_ + angular * 0.5; + // Compute intermediate value of the heading + const double theta_mid = heading_ + omega_bz * 0.5 * dt; - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; + // Use the intermediate values to update the state + x_ += v_bx * cos(theta_mid) * dt; + y_ += v_bx * sin(theta_mid) * dt; + heading_ += omega_bz * dt; } -/** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ -void SteeringOdometry::integrate_exact(double linear, double angular) +void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) { - if (fabs(angular) < 1e-6) + const double delta_x_b = v_bx * dt; + const double delta_theta = omega_bz * dt; + + if (fabs(delta_theta) < 1e-6) { - integrate_runge_kutta_2(linear, angular); + /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): + integrate_runge_kutta_2(v_bx, omega_bz, dt); } else { - /// Exact integration (should solve problems when angular is zero): + /// Exact integration const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); + const double R = delta_x_b / delta_theta; + heading_ += delta_theta; + x_ += R * (sin(heading_) - sin(heading_old)); + y_ += -R * (cos(heading_) - cos(heading_old)); } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index f89d78a52c..03be6b88f0 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,24 +60,28 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } } } From 615524831a3bd3e0fc4e4678c6eb9afd7f39f326 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jun 2024 10:21:17 +0200 Subject: [PATCH 504/524] [Steering controllers library] Reference interfaces are body twist (#1168) --- .../test/test_ackermann_steering_controller.cpp | 2 +- .../test/test_ackermann_steering_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- steering_controllers_library/doc/userdoc.rst | 4 +++- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 4 ++-- .../test/test_steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 13 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..38cca0cac9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -84,7 +84,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49331ae8a2..363db793d5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -302,7 +302,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..fd8565853f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -68,7 +68,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 390447c25f..dcdb7ed169 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -266,7 +266,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index a52e34c120..44b180162e 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -56,7 +56,9 @@ References (from a preceding controller) Used when controller is in chained mode (``in_chained_mode == true``). - ``/linear/velocity`` double, in m/s -- ``/angular/position`` double, in rad +- ``/angular/velocity`` double, in rad/s + +representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c11d90dc6f..e417b9fcd1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -131,7 +131,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // name constants for reference interfaces size_t nr_ref_itfs_; - // store last velocity + // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5880000d17..5445196a6e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -324,7 +324,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; @@ -396,7 +396,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store and set commands + // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..98ab97fdc3 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -66,7 +66,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 05ac17b454..b6fe1770c1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -324,7 +324,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 1807ab59a8..b4d58a95c0 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -285,7 +285,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From b81be48fb1b6101a3c601c0ea11c5393e5a7026e Mon Sep 17 00:00:00 2001 From: Enrique Llorente Pastora Date: Wed, 19 Jun 2024 19:31:22 +0200 Subject: [PATCH 505/524] [STEERING] Add missing `tan` call for ackermann (#1117) --- steering_controllers_library/src/steering_odometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 32cdb69454..0d12518a3d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -123,7 +123,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -136,7 +136,7 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -148,7 +148,7 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } From dc928985b34c8bf439bc92f210d26209d09f262c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 1 Jul 2024 13:52:30 +0200 Subject: [PATCH 506/524] Bump version of pre-commit hooks (#1185) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c75acbd76d..29862f70e4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.15.2 + rev: v3.16.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -56,14 +56,14 @@ repos: args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.0.0 + rev: 7.1.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.5 + rev: v18.1.8 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.4 + rev: 0.28.6 hooks: - id: check-github-workflows args: ["--verbose"] From 6439a0ded6d0072af0be710cd371a6897e5924a9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:03:41 +0100 Subject: [PATCH 507/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 7 +++++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 7 +++++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 8 ++++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 7 +++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 84 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 5cfdefb83b..165573dc84 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1a2dd8ed96..ce1aa256f1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 8c3d27e24f..52c41f549c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a4473f0f83..6be80a2e54 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 63b44cd218..eb619ca1ba 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index deca56a197..005a23c0e4 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3f1917dcad..1db9c16874 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e0b5a0a311..e7a8105cd2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index cebfc8d78c..96141e51e1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ec9b79726f..8582c56d4f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 544cf500ef..4d7071701b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove manual angle-wraparound parameter (`#1152 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * JTC trajectory end time validation fix (`#1090 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 4b884dc386..58bdcd3691 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index f454aadc4e..85c48140ff 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7dd295970d..a689b4fda5 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index af872ec8a3..5fcde17af9 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 19ead3093f..99553224b7 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 121a5066c2..96cab6e11c 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * [RQT-JTC] limits from jtc controlled joints (`#1146 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e1cdc069df..8a16a89fc4 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Enrique Llorente Pastora, Quique Llorente + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 8f36bae23c..231774c823 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f6423146a0..901ce56631 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 64a09e4309..ca954bb079 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ From b441ced4c19dba79067239e7a28751adad298640 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:04:06 +0100 Subject: [PATCH 508/524] 4.10.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 165573dc84..e27cac2205 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 7b66dd765b..77555c1f01 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.9.0 + 4.10.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ce1aa256f1..a3faf30ab1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2add40baf7..9abdb5cf82 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.9.0 + 4.10.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 52c41f549c..a8956560b5 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index bcad405abe..b49b9f1a23 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6be80a2e54..5b9275359d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8765d2506f..9cfdee9a29 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.9.0 + 4.10.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index eb619ca1ba..9b1240263b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6519a07b07..faf31ada51 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 005a23c0e4..8f978cfd08 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4576bbb8b0..ae0c89dcf2 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 1db9c16874..75926feff6 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index b4f34fce8c..1e24ca0201 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e7a8105cd2..adf9f8e208 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index f4ee00591a..5a9ff6ed87 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.9.0 + 4.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 96141e51e1..6649777076 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 032e5addc1..383c90d89a 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8582c56d4f..e4f73e5cc6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a34c05b269..9ca30f4e52 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.9.0 + 4.10.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 4d7071701b..2c044b28b0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove manual angle-wraparound parameter (`#1152 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 2c04d7fa23..733059736a 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.9.0 + 4.10.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 58bdcd3691..101d507f56 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index be598f94ae..3bd87c1875 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.9.0 + 4.10.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 85c48140ff..0582a40895 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 37bcdbe108..013b60c790 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index a689b4fda5..7c671ae3d3 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 31e8b1755b..da74b2d62f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5fcde17af9..1db9ae1c05 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 278ecb0b1a..14f686cce4 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.9.0 + 4.10.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 99553224b7..c483ee7c2f 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c321c9703d..2c26e5cfe0 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.9.0 + 4.10.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e0dfb4d65e..bb9ff7b303 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.9.0", + version="4.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 96cab6e11c..bd2b00a1fe 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 68a173217a..4d42c2d00d 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.9.0 + 4.10.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index e9a19034c6..b966635801 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.9.0", + version="4.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8a16a89fc4..ee21fed403 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 08d252d794..5b55f7f9de 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.9.0 + 4.10.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 231774c823..4034036e08 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 51fe867106..cdb62117fd 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.9.0 + 4.10.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 901ce56631..c80b90ff0f 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 6e1ed66edd..f2408123df 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ca954bb079..c4d146a21d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 05f6245686..75c05fab94 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From df0449263f1568c4340a4e712ffb8d8412e06a0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:43:19 +0200 Subject: [PATCH 509/524] Fix steering controllers library kinematics (#1150) --- .../test_ackermann_steering_controller.cpp | 6 +- .../test/test_bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 17 ++- .../src/steering_controllers_library.cpp | 2 +- .../src/steering_odometry.cpp | 82 ++++++++--- .../test/test_steering_odometry.cpp | 127 +++++++++++++++--- .../test_tricycle_steering_controller.cpp | 3 + 7 files changed, 197 insertions(+), 50 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 38cca0cac9..718e6f5856 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR( + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( @@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index fd8565853f..9904f791b6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -237,7 +237,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command[0], 1.1); EXPECT_EQ(msg.steering_angle_command[0], 2.2); - publish_commands(); + publish_commands(0.1, 0.2); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( @@ -245,14 +245,14 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 0104b011c7..dadc5730d4 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,6 +18,7 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#include #include #include @@ -36,6 +37,9 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -188,10 +192,11 @@ class SteeringOdometry * \brief Calculates inverse kinematics for the desired linear and angular velocities * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz); + const double v_bx, const double omega_bz, const bool open_loop = true); /** * \brief Reset poses, heading, and accumulators @@ -230,6 +235,16 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + /** * \brief Reset linear and angular accumulators */ diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5445196a6e..1aef556212 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -401,7 +401,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_angular_velocity_ = reference_interfaces_[1]; auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0d12518a3d..f8b42771e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace steering_odometry { @@ -128,13 +129,26 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } +double SteeringOdometry::get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + // overdetermined, we take the average + double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius + wheel_track_ * 0.5); + double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; +} + bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; @@ -145,10 +159,18 @@ bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -181,30 +203,41 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (omega_bz == 0 || v_bx == 0) + if (fabs(v_bx) < std::numeric_limits::epsilon()) { - return 0; + // avoid division by zero + return 0.; } return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz) + const double v_bx, const double omega_bz, const bool open_loop) { // desired wheel speed and steering angle of the middle of traction and steering axis - double Ws, phi; + double Ws, phi, phi_IK = steer_pos_; +#if 0 if (v_bx == 0 && omega_bz != 0) { - // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + // TODO(anyone) this would be only possible if traction is on the steering axis phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); - Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; } + // wheel speed + Ws = v_bx / wheel_radius_; if (config_type_ == BICYCLE_CONFIG) { @@ -216,17 +249,20 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } + // simple steering steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } @@ -234,14 +270,16 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; + // shortcut, no steering steering_commands = {phi, phi}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; @@ -279,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2( const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(theta_mid) * dt; - y_ += v_bx * sin(theta_mid) * dt; + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; heading_ += omega_bz * dt; } @@ -289,7 +327,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double delta_x_b = v_bx * dt; const double delta_theta = omega_bz * dt; - if (fabs(delta_theta) < 1e-6) + if (is_close_to_zero(delta_theta)) { /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): integrate_runge_kutta_2(v_bx, omega_bz, dt); @@ -300,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double heading_old = heading_; const double R = delta_x_b / delta_theta; heading_ += delta_theta; - x_ += R * (sin(heading_) - sin(heading_old)); - y_ += -R * (cos(heading_) - cos(heading_old)); + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } } diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..d93e29eca5 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,13 +78,13 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); - auto cmd = odom.get_commands(1., 0.); + auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -79,13 +93,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., 0.1); + auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -94,13 +108,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., -0.1); + auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); @@ -109,6 +123,47 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -121,25 +176,57 @@ TEST(TestSteeringOdometry, bicycle_odometry) EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } -TEST(TestSteeringOdometry, tricycle_odometry) +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) { steering_odometry::SteeringOdometry odom(1); - odom.set_wheel_params(1., 1., 1.); + odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); - EXPECT_NEAR(odom.get_angular(), .1, 1e-3); - EXPECT_NEAR(odom.get_x(), .1, 1e-3); - EXPECT_NEAR(odom.get_heading(), .01, 1e-3); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering } -TEST(TestSteeringOdometry, ackermann_odometry) +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..328f5e4d6a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -201,6 +201,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -246,6 +247,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -258,6 +260,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( From 07061f96f21fd4436a1f48b29e74beb21be8709a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:56:47 +0200 Subject: [PATCH 510/524] [JTC] Process tolerances sent with action goal (#716) --- doc/migration/Jazzy.rst | 1 + doc/release_notes/Jazzy.rst | 14 + joint_trajectory_controller/CMakeLists.txt | 4 + joint_trajectory_controller/doc/userdoc.rst | 16 +- .../joint_trajectory_controller.hpp | 3 + .../tolerances.hpp | 219 ++++++++- .../src/joint_trajectory_controller.cpp | 55 ++- .../test/test_tolerances.cpp | 424 ++++++++++++++++++ .../test/test_trajectory_actions.cpp | 178 +++++++- .../test/test_trajectory_controller_utils.hpp | 41 +- 10 files changed, 921 insertions(+), 34 deletions(-) create mode 100644 joint_trajectory_controller/test/test_tolerances.cpp diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst index 72e6a52e6d..2114436098 100644 --- a/doc/migration/Jazzy.rst +++ b/doc/migration/Jazzy.rst @@ -18,3 +18,4 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. +* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4c5db18f3e..4793fd957d 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -33,6 +33,20 @@ joint_trajectory_controller * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. pid_controller ************************ diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..46c245be9c 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -61,6 +61,10 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index af495ad14d..4dcb71a064 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -152,7 +152,21 @@ Actions [#f1]_ The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: + +.. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ae370df0a6..c3fbb58456 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -244,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c46b1c297f..1998930182 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,12 +30,15 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" namespace joint_trajectory_controller { @@ -85,16 +88,19 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -106,16 +112,221 @@ SegmentTolerances get_segment_tolerances(Params const & params) tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param jtc_logger The logger to use for output + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param joints The joints configured by ROS parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances(default_tolerances); + + active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (joint_tol.position > 0.0) + { + active_tolerances.state_tolerance[i].position = joint_tol.position; + } + else if (is_erase_value(joint_tol.position)) + { + active_tolerances.state_tolerance[i].position = 0.0; + } + else if (joint_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.velocity > 0.0) + { + active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; + } + else if (is_erase_value(joint_tol.velocity)) + { + active_tolerances.state_tolerance[i].velocity = 0.0; + } + else if (joint_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.acceleration > 0.0) + { + active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; + } + else if (is_erase_value(joint_tol.acceleration)) + { + active_tolerances.state_tolerance[i].acceleration = 0.0; + } + else if (joint_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (goal_tol.position > 0.0) + { + active_tolerances.goal_state_tolerance[i].position = goal_tol.position; + } + else if (is_erase_value(goal_tol.position)) + { + active_tolerances.goal_state_tolerance[i].position = 0.0; + } + else if (goal_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.velocity > 0.0) + { + active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; + } + else if (is_erase_value(goal_tol.velocity)) + { + active_tolerances.goal_state_tolerance[i].velocity = 0.0; + } + else if (goal_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.acceleration > 0.0) + { + active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; + } + else if (is_erase_value(goal_tol.acceleration)) + { + active_tolerances.goal_state_tolerance[i].acceleration = 0.0; + } + else if (goal_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b80d1b7908..31598bb813 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -126,11 +126,12 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) @@ -200,6 +201,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -207,7 +209,7 @@ controller_interface::return_type JointTrajectoryController::update( !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -224,8 +226,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], - true /* show_errors */)) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -233,14 +234,14 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], - false /* show_errors */)) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; // print once, goal will be aborted afterwards @@ -320,7 +321,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -339,7 +340,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); @@ -358,7 +359,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); + RCLCPP_WARN(logger, error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -368,7 +369,7 @@ controller_interface::return_type JointTrajectoryController::update( else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -376,7 +377,7 @@ controller_interface::return_type JointTrajectoryController::update( else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -620,11 +621,11 @@ void JointTrajectoryController::query_state_service( controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -782,6 +783,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -873,6 +876,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -880,7 +885,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( params_ = param_listener_->get_params(); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // order all joints in the storage for (const auto & interface : params_.command_interfaces) @@ -892,8 +897,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -906,8 +911,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -949,9 +954,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // deactivate timeout RCLCPP_WARN( - get_node()->get_logger(), - "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, - default_tolerances_.goal_time_tolerance); + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); cmd_timeout_ = 0.0; } } @@ -1173,6 +1177,11 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..66914b6da4 --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,424 @@ +// Copyright 2024 Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double timeout, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 332a30c53a..fb749ac250 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -31,7 +31,6 @@ #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -44,10 +43,12 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -152,10 +153,16 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -488,6 +495,165 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SCOPED_TRACE("Check default values"); + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } +} + TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters @@ -682,7 +848,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -732,7 +899,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f049bc65a9..693e19e67a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" namespace { @@ -38,11 +39,44 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); } - } // namespace namespace test_trajectory_controllers @@ -138,6 +172,11 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + std::vector get_pids() const { return pids_; } joint_trajectory_controller::SegmentTolerances get_tolerances() const From 991ef48b79a12974a1b37763e818842906e3553b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jul 2024 23:08:20 +0200 Subject: [PATCH 511/524] added changes corresponding to the logger and clock propagation in ResourceManager (#1184) --- .../test/test_load_ackermann_steering_controller.cpp | 4 +--- .../test/test_load_admittance_controller.cpp | 4 +--- .../test/test_load_bicycle_steering_controller.cpp | 4 +--- .../test/test_load_diff_drive_controller.cpp | 3 +-- .../test/test_load_joint_group_effort_controller.cpp | 4 +--- .../test/test_load_force_torque_sensor_broadcaster.cpp | 4 +--- .../test/test_load_forward_command_controller.cpp | 4 +--- .../test_load_multi_interface_forward_command_controller.cpp | 4 +--- .../test/test_load_gripper_action_controllers.cpp | 4 +--- .../test/test_load_imu_sensor_broadcaster.cpp | 4 +--- .../test/test_load_joint_state_broadcaster.cpp | 4 +--- .../test/test_load_joint_trajectory_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 5 ++--- .../test/test_load_joint_group_position_controller.cpp | 4 +--- .../test/test_load_range_sensor_broadcaster.cpp | 4 +--- tricycle_controller/test/test_load_tricycle_controller.cpp | 4 +--- .../test/test_load_tricycle_steering_controller.cpp | 4 +--- .../test/test_load_joint_group_velocity_controller.cpp | 4 +--- 18 files changed, 19 insertions(+), 53 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 0a8cd7b80c..101ecce8ff 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 23be1f23f5..69808f3505 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,9 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 955feb33c5..f3828660a5 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 4c9d2f984f..0c65532c38 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,8 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique(ros2_control_test_assets::diffbot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 52f1f9934a..4008ac8534 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 0c269d6a31..2e87505003 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index b493e52b2a..c192f1eb5f 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 41a9b74698..52b63bdae8 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 0ef5f0bcb2..5641e1b4b3 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,9 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index f4e6105ed6..a477a731ff 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 5efb587805..266e33c1c8 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index eb1a3691e6..b3635efaca 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 3a75f6e170..ed645b872c 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,9 +33,8 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index bc27b5e629..950773351c 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 5c400bef91..1d8a55b932 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bd54459780..bb4194909c 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,9 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 0d04afdf38..4fa3698409 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index e426349f96..7070594639 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 776c432dbc9a8db11119823803daad9525c63846 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 9 Jul 2024 09:08:28 +0200 Subject: [PATCH 512/524] [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (#1192) --- .../tolerances.hpp | 186 +++++++----------- .../test/test_tolerances.cpp | 35 +++- .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 99 insertions(+), 123 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 1998930182..c4404920df 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -31,6 +31,7 @@ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #include +#include #include #include @@ -126,6 +127,33 @@ SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Para return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + /** * \brief Populate trajectory segment tolerances using data from an action goal. * @@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances( const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); - - active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); static auto logger = jtc_logger.get_child("tolerance"); - RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); - // from - // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg - // There are two special values for tolerances: - // * 0 - The tolerance is unspecified and will remain at whatever the default is - // * -1 - The tolerance is "erased". - // If there was a default, the joint will be allowed to move without restriction. - constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) - { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "Specified illegal goal_time_tolerance: " + << rclcpp::Duration(goal.goal_time_tolerance).seconds() + << ". Using default tolerances"); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -173,60 +203,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (joint_tol.position > 0.0) - { - active_tolerances.state_tolerance[i].position = joint_tol.position; - } - else if (is_erase_value(joint_tol.position)) + std::string interface = ""; + try { - active_tolerances.state_tolerance[i].position = 0.0; + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); } - else if (joint_tol.position < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.velocity > 0.0) - { - active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; - } - else if (is_erase_value(joint_tol.velocity)) - { - active_tolerances.state_tolerance[i].velocity = 0.0; - } - else if (joint_tol.velocity < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.acceleration > 0.0) - { - active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; - } - else if (is_erase_value(joint_tol.acceleration)) - { - active_tolerances.state_tolerance[i].acceleration = 0.0; - } - else if (joint_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } @@ -256,60 +250,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (goal_tol.position > 0.0) - { - active_tolerances.goal_state_tolerance[i].position = goal_tol.position; - } - else if (is_erase_value(goal_tol.position)) - { - active_tolerances.goal_state_tolerance[i].position = 0.0; - } - else if (goal_tol.position < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.velocity > 0.0) - { - active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; - } - else if (is_erase_value(goal_tol.velocity)) + std::string interface = ""; + try { - active_tolerances.goal_state_tolerance[i].velocity = 0.0; + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); } - else if (goal_tol.velocity < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.acceleration > 0.0) - { - active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; - } - else if (is_erase_value(goal_tol.acceleration)) - { - active_tolerances.goal_state_tolerance[i].acceleration = 0.0; - } - else if (goal_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 66914b6da4..af5036b869 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -183,7 +183,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) path_tolerance.push_back(tolerance); goal_tolerance.push_back(tolerance); - auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); @@ -215,6 +215,31 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) // send goal with invalid tolerances, are the default ones used? TEST_F(TestTolerancesFixture, test_invalid_tolerances) { + { + SCOPED_TRACE("negative goal_time_tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } { SCOPED_TRACE("negative path position tolerance"); std::vector points; @@ -238,7 +263,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -264,7 +288,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -290,7 +313,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } } @@ -395,7 +414,6 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) @@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 693e19e67a..bbe12251a1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -45,6 +45,7 @@ const double stopped_velocity_tolerance = 0.1; [[maybe_unused]] void expectDefaultTolerances( joint_trajectory_controller::SegmentTolerances active_tolerances) { + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From a0d844f053d22b19045f51c38a2fa09d98148c89 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:14 +0100 Subject: [PATCH 513/524] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 21 files changed, 104 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e27cac2205..84ac53044c 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a3faf30ab1..e75a71a8af 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a8956560b5..b1e1bf832f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5b9275359d..c402e58344 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 9b1240263b..6f7c2771cb 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8f978cfd08..a7392ad187 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 75926feff6..d37dd5f30d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index adf9f8e208..acd4c2534c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6649777076..46ae9e4d27 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e4f73e5cc6..f20ca59557 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2c044b28b0..23838b7e3f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* [JTC] Process tolerances sent with action goal (`#716 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove manual angle-wraparound parameter (`#1152 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 101d507f56..d5d140339b 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 0582a40895..d5f8b52cbb 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7c671ae3d3..6be8a70b9d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1db9ae1c05..39c00e3c85 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c483ee7c2f..d529a740b3 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index bd2b00a1fe..2ffbb95666 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ee21fed403..7ea9c5d893 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich + 4.10.0 (2024-07-01) ------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 4034036e08..feb9b105cb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index c80b90ff0f..a34e991d2a 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c4d146a21d..e749d6f8a0 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- From 18ce11a42d1549678b418794d283ea80914c4a6b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:31 +0100 Subject: [PATCH 514/524] 4.11.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 84ac53044c..21840061d4 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 77555c1f01..dfa733515f 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.10.0 + 4.11.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index e75a71a8af..4ae5ea39f8 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9abdb5cf82..d6e13b0634 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.10.0 + 4.11.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index b1e1bf832f..831aa5a2b3 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index b49b9f1a23..7afcf53706 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c402e58344..00f777df67 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9cfdee9a29..d7fa06b762 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.10.0 + 4.11.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6f7c2771cb..70cfaad021 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index faf31ada51..a7744927a1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a7392ad187..eac8c9959a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index ae0c89dcf2..90fcb3a684 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d37dd5f30d..17196fed65 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1e24ca0201..f4c02d8de7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index acd4c2534c..968a1088c2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 5a9ff6ed87..72d2ec48f2 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.10.0 + 4.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 46ae9e4d27..5bacde755e 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 383c90d89a..5f0e981605 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f20ca59557..cc01373289 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9ca30f4e52..77bbe2ca00 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.10.0 + 4.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 23838b7e3f..047d232155 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * [JTC] Process tolerances sent with action goal (`#716 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 733059736a..d32d4e3daf 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.10.0 + 4.11.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index d5d140339b..8e45f042f0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 3bd87c1875..2a44c9ad1b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.10.0 + 4.11.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d5f8b52cbb..c9e2c749b5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 013b60c790..33f0f0c15e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 6be8a70b9d..31458c07d6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index da74b2d62f..e4525f53fd 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 39c00e3c85..a54d39b85d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 14f686cce4..24e4429ade 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.10.0 + 4.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d529a740b3..78e18dffaa 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2c26e5cfe0..87af6e6695 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.10.0 + 4.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index bb9ff7b303..91db299739 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.10.0", + version="4.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 2ffbb95666..20450f3ca3 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4d42c2d00d..85c44c58fe 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.10.0 + 4.11.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index b966635801..2588a47cc8 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.10.0", + version="4.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 7ea9c5d893..d4f62b806c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5b55f7f9de..7d172d45a8 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.10.0 + 4.11.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index feb9b105cb..fdde4481e2 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cdb62117fd..20310e6b9a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.10.0 + 4.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a34e991d2a..6f4ad5d8c1 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f2408123df..4aeb8a086f 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e749d6f8a0..614e023984 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 75c05fab94..d128f2b45b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 91ebb73b23872f60e09a0d3826b57d0f8b3bf5b7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 11 Jul 2024 07:38:47 -0700 Subject: [PATCH 515/524] Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (#1002) --- doc/controllers_index.rst | 1 + .../gripper_action_controller_impl.hpp | 5 + parallel_gripper_controller/CMakeLists.txt | 84 ++++ parallel_gripper_controller/doc/userdoc.rst | 19 + .../parallel_gripper_action_controller.hpp | 174 ++++++++ ...arallel_gripper_action_controller_impl.hpp | 396 ++++++++++++++++++ .../visibility_control.hpp | 56 +++ parallel_gripper_controller/package.xml | 35 ++ .../ros_control_plugins.xml | 10 + .../gripper_action_controller_parameters.yaml | 69 +++ .../parallel_gripper_action_controller.cpp | 25 ++ ...oad_parallel_gripper_action_controller.cpp | 45 ++ .../test/test_parallel_gripper_controller.cpp | 169 ++++++++ .../test/test_parallel_gripper_controller.hpp | 66 +++ 14 files changed, 1154 insertions(+) create mode 100644 parallel_gripper_controller/CMakeLists.txt create mode 100644 parallel_gripper_controller/doc/userdoc.rst create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp create mode 100644 parallel_gripper_controller/package.xml create mode 100644 parallel_gripper_controller/ros_control_plugins.xml create mode 100644 parallel_gripper_controller/src/gripper_action_controller_parameters.yaml create mode 100644 parallel_gripper_controller/src/parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.hpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2cb55150ce..78abe5d21f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -52,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 77598591ae..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() template controller_interface::CallbackReturn GripperActionController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `position_controllers/GripperActionController` and " + "`effort_controllers::GripperActionController` controllers are replaced by " + "'parallel_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..56aa623a6f --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(parallel_gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + +add_library(parallel_gripper_action_controller SHARED + src/parallel_gripper_action_controller.cpp +) +target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(parallel_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5c04eb5f90 --- /dev/null +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst + +.. _parallel_gripper_controller_userdoc: + +Parallel Gripper Action Controller +----------------------------------- + +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp new file mode 100644 index 0000000000..d303990b89 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -0,0 +1,174 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/parallel_gripper_command.hpp" + +// rclcpp_action +#include "rclcpp_action/create_server.hpp" + +// ros_controls +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "parallel_gripper_action_controller_parameters.hpp" + +namespace parallel_gripper_action_controller +{ +/** + * \brief Controller for executing a `ParallelGripperCommand` action. + * + * \tparam HardwareInterface Controller hardware interface. Currently \p + * hardware_interface::HW_IF_POSITION and \p + * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. + */ + +class GripperActionController : public controller_interface::ControllerInterface +{ +public: + /** + * \brief Store position and max effort in struct to allow easier realtime + * buffer usage + */ + struct Commands + { + double position_cmd_; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the + * position and velocity state interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; + using ActionServer = rclcpp_action::Server; + using ActionServerPtr = ActionServer::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + bool update_hold_position_; + + bool verbose_ = false; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::optional> + joint_command_interface_; + std::optional> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; + + rclcpp::Duration action_monitor_period_; + + // ROS API + ActionServerPtr action_server_; + + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + + void accepted_callback(std::shared_ptr goal_handle); + + void preempt_active_goal(); + + void set_hold_position(); + + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command + + /** + * \brief Check for success and publish appropriate result and feedback. + **/ + void check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); +}; + +} // namespace parallel_gripper_action_controller + +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..b6fccf1bbf --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -0,0 +1,396 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include +#include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace parallel_gripper_action_controller +{ + +void GripperActionController::preempt_active_goal() +{ + // Cancels the currently active goal + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + // Marks the current goal as canceled + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + command_struct_rt_ = *(command_.readFromRT()); + + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double error_position = command_struct_rt_.position_cmd_ - current_position; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) +{ + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionController::accepted_callback( + std::shared_ptr goal_handle) // Try to update goal +{ + auto rt_goal = std::make_shared(goal_handle); + + // Accept new goal + preempt_active_goal(); + + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + { + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + } + else + { + command_struct_.max_velocity_ = params_.max_velocity; + } + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } + command_.writeFromNonRT(command_struct_); + + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +rclcpp_action::CancelResponse GripperActionController::cancel_callback( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Enter hold current position mode + set_hold_position(); + + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + // Reset current goal + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.writeFromNonRT(command_struct_); +} + +void GripperActionController::check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) + { + return; + } + + if (fabs(error_position) < params_.goal_tolerance) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = true; + pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + else + { + if (fabs(current_velocity) > params_.stall_velocity_threshold) + { + last_movement_time_ = time; + } + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = true; + + if (params_.allow_stalling) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + active_goal->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + active_goal->setAborted(pre_alloc_result_); + } + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + // Action status checking update rate + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + RCLCPP_INFO_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) +{ + auto command_interface_it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto position_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (position_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (position_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto velocity_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); + if (velocity_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (velocity_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + + joint_command_interface_ = *command_interface_it; + joint_position_state_interface_ = *position_state_interface_it; + joint_velocity_state_interface_ = *velocity_state_interface_it; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "set_gripper_max_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "set_gripper_max_velocity") + { + speed_interface_ = interface; + } + } + + + // Command - non RT version + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + // Action interface + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_cmd", + std::bind( + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), + std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + joint_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (!params_.max_effort_interface.empty()) + { + names.push_back({params_.max_effort_interface}); + } + if (!params_.max_velocity_interface.empty()) + { + names.push_back({params_.max_velocity_interface}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace parallel_gripper_action_controller + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..63e5c8e1c7 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml new file mode 100644 index 0000000000..f189071617 --- /dev/null +++ b/parallel_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + parallel_gripper_controller + 2.32.0 + The parallel_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9b0dbb8a05 --- /dev/null +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,69 @@ +parallel_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + read_only: true, + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + read_only: true, + } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 0.0 ] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, + } diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..3a6d028ddf --- /dev/null +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -0,0 +1,25 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta + +// Project +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + parallel_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..130b12e0bb --- /dev/null +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -0,0 +1,45 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGripperActionControllers, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); + + rclcpp::shutdown(); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp new file mode 100644 index 0000000000..d9a5ae958b --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_parallel_gripper_controller.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::ParallelGripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +void GripperControllerTest::SetUpController() +{ + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp new file mode 100644 index 0000000000..8bc6ab954c --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController : public parallel_gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ From ebca6c0a3f29d75521a7a85683206b32073bd8ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jul 2024 12:02:19 +0200 Subject: [PATCH 516/524] Fix parallel gripper controller CI (#1202) --- .../test/test_load_ackermann_steering_controller.cpp | 2 +- .../test/test_load_admittance_controller.cpp | 2 +- .../test/test_load_bicycle_steering_controller.cpp | 2 +- .../test/test_load_diff_drive_controller.cpp | 2 +- .../test/test_load_joint_group_effort_controller.cpp | 2 +- .../test/test_load_force_torque_sensor_broadcaster.cpp | 2 +- .../test_load_multi_interface_forward_command_controller.cpp | 2 +- .../test/test_load_gripper_action_controllers.cpp | 2 +- .../test/test_load_imu_sensor_broadcaster.cpp | 2 +- .../test/test_load_joint_state_broadcaster.cpp | 2 +- .../test/test_load_joint_trajectory_controller.cpp | 2 +- .../parallel_gripper_action_controller_impl.hpp | 1 - .../test/test_load_parallel_gripper_action_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 3 +-- .../test/test_load_joint_group_position_controller.cpp | 2 +- .../test/test_load_range_sensor_broadcaster.cpp | 2 +- tricycle_controller/test/test_load_tricycle_controller.cpp | 2 +- .../test/test_load_tricycle_steering_controller.cpp | 2 +- .../test/test_load_joint_group_velocity_controller.cpp | 2 +- 19 files changed, 18 insertions(+), 22 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 101ecce8ff..8e81643835 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 69808f3505..ea4c14caac 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,7 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index f3828660a5..7be5e2dd1d 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 0c65532c38..bcd334631f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 4008ac8534..0ac1053d18 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 2e87505003..f8b367d341 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 52b63bdae8..49bc277824 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 5641e1b4b3..b607fb6fe7 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,7 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index a477a731ff..fa2afab200 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 266e33c1c8..b3cf778063 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index b3635efaca..5b36afc4e8 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index b6fccf1bbf..bcfbb3b5c5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -322,7 +322,6 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - non RT version command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 130b12e0bb..6ba99ac248 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index ed645b872c..5cfb04e860 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,8 +33,7 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index 950773351c..f71474bd4a 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 1d8a55b932..b416ac4ef9 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bb4194909c..486c04515b 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,7 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 4fa3698409..210153eef8 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 7070594639..a7dc72f337 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 362067bc269024faa7a866d13dfc17050b390ce8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 Jul 2024 23:56:03 +0200 Subject: [PATCH 517/524] Use a single file for migration + release notes (#1204) --- doc/{migration/Jazzy.rst => migration.rst} | 0 doc/{release_notes/Jazzy.rst => release_notes.rst} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename doc/{migration/Jazzy.rst => migration.rst} (100%) rename doc/{release_notes/Jazzy.rst => release_notes.rst} (100%) diff --git a/doc/migration/Jazzy.rst b/doc/migration.rst similarity index 100% rename from doc/migration/Jazzy.rst rename to doc/migration.rst diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes.rst similarity index 100% rename from doc/release_notes/Jazzy.rst rename to doc/release_notes.rst From 12b531082bcea252d1c4359dacf648afa3a9568d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jul 2024 11:47:00 +0200 Subject: [PATCH 518/524] [doc] Fix controller idx page (#1205) * Fix controller idx page * Fix gh links --- doc/controllers_index.rst | 4 ++-- doc/migration.rst | 4 ++-- doc/release_notes.rst | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 78abe5d21f..683f23c202 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -16,9 +16,9 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - :glob: - * + mobile_robot_kinematics.rst + writing_new_controller.rst Controllers for Wheeled Mobile Robots diff --git a/doc/migration.rst b/doc/migration.rst index 2114436098..4c0e4608d6 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst -Iron to Jazzy +Migration Guides: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4793fd957d..b55db861b3 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst -Iron to Jazzy +Release Notes: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes the changes between Iron (previous) and Jazzy (current) releases. From 0bb88805713fb21cddc418bc4c502960d9fd9f39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 16 Jul 2024 10:02:42 +0200 Subject: [PATCH 519/524] [JTC] Fix test_tolerances_via_actions (#1209) --- joint_trajectory_controller/test/test_tolerances.cpp | 4 ++-- .../test/test_trajectory_actions.cpp | 11 +++++------ .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index af5036b869..bd6304df29 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -32,14 +32,14 @@ using trajectory_msgs::msg::JointTrajectoryPoint; std::vector joint_names_ = {"joint1", "joint2", "joint3"}; control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( - const std::vector & points, double timeout, + const std::vector & points, double goal_time_tolerance, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fb749ac250..3e65016403 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -153,14 +153,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector & points, double goal_time_tolerance, + const GoalOptions & opt, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; @@ -529,7 +530,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -539,7 +540,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } @@ -639,7 +639,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -649,7 +649,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bbe12251a1..1750f7d30d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -46,6 +46,7 @@ const double stopped_velocity_tolerance = 0.1; joint_trajectory_controller::SegmentTolerances active_tolerances) { EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From b958da39e04ac9a560b68f4361807dcdc0c77ec6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 11:24:08 +0200 Subject: [PATCH 520/524] [GripperController] Fix failing tests (#1210) --- parallel_gripper_controller/CMakeLists.txt | 14 ++--- ...arallel_gripper_action_controller_impl.hpp | 2 +- .../gripper_action_controller_parameters.yaml | 3 ++ .../gripper_action_controller_params.yaml | 7 +++ ...oad_parallel_gripper_action_controller.cpp | 16 +++--- .../test/test_parallel_gripper_controller.cpp | 54 +++++++++---------- .../test/test_parallel_gripper_controller.hpp | 3 +- 7 files changed, 57 insertions(+), 42 deletions(-) create mode 100644 parallel_gripper_controller/test/gripper_action_controller_params.yaml diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 56aa623a6f..75380a953f 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -48,17 +48,19 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_parallel_gripper_action_controllers - test/test_load_parallel_gripper_action_controller.cpp - ) + add_rostest_with_parameters_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) ament_target_dependencies(test_load_parallel_gripper_action_controllers controller_manager ros2_control_test_assets ) - ament_add_gmock(test_parallel_gripper_controller - test/test_parallel_gripper_controller.cpp - ) + add_rostest_with_parameters_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_parallel_gripper_controller PRIVATE include) target_link_libraries(test_parallel_gripper_controller parallel_gripper_action_controller ) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index bcfbb3b5c5..d37cb87fc8 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -44,7 +44,6 @@ controller_interface::CallbackReturn GripperActionController::on_init() try { param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -245,6 +244,7 @@ controller_interface::CallbackReturn GripperActionController::on_configure( RCLCPP_ERROR(logger, "Joint name cannot be empty"); return controller_interface::CallbackReturn::ERROR; } + RCLCPP_INFO(logger, "Joint name is : %s", params_.joint.c_str()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml index 9b0dbb8a05..10b0df0958 100644 --- a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -11,6 +11,9 @@ parallel_gripper_action_controller: joint: { type: string, read_only: true, + validation: { + not_empty<>: null + } } state_interfaces: { type: string_array, diff --git a/parallel_gripper_controller/test/gripper_action_controller_params.yaml b/parallel_gripper_controller/test/gripper_action_controller_params.yaml new file mode 100644 index 0000000000..521960e18c --- /dev/null +++ b/parallel_gripper_controller/test/gripper_action_controller_params.yaml @@ -0,0 +1,7 @@ +test_gripper_action_position_controller: + ros__parameters: + joint: "joint1" + +test_gripper_action_position_controller_empty_joint: + ros__parameters: + joint: "" diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 6ba99ac248..35a10a8c31 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -22,8 +22,6 @@ TEST(TestLoadGripperActionControllers, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -32,12 +30,16 @@ TEST(TestLoadGripperActionControllers, load_controller) ASSERT_NE( cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController"), - nullptr); - ASSERT_NE( - cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + "test_gripper_action_position_controller", + "parallel_gripper_action_controller/GripperActionController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index d9a5ae958b..417d87d40f 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -34,9 +34,9 @@ using GoalHandle = rclcpp_action::ServerGoalHandle; using testing::SizeIs; using testing::UnorderedElementsAre; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void GripperControllerTest::SetUpTestCase() {} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +void GripperControllerTest::TearDownTestCase() {} void GripperControllerTest::SetUp() { @@ -46,11 +46,13 @@ void GripperControllerTest::SetUp() void GripperControllerTest::TearDown() { controller_.reset(nullptr); } -void GripperControllerTest::SetUpController() +void GripperControllerTest::SetUpController( + const std::string & controller_name = "test_gripper_action_position_controller", + controller_interface::return_type expected_result = controller_interface::return_type::OK) { const auto result = - controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, expected_result); std::vector command_ifs; command_ifs.emplace_back(this->joint_1_cmd_); @@ -62,7 +64,9 @@ void GripperControllerTest::SetUpController() TEST_F(GripperControllerTest, ParametersNotSet) { - this->SetUpController(); + this->SetUpController( + "test_gripper_action_position_controller_no_parameters", + controller_interface::return_type::ERROR); // configure failed, 'joints' parameter not set ASSERT_EQ( @@ -72,9 +76,9 @@ TEST_F(GripperControllerTest, ParametersNotSet) TEST_F(GripperControllerTest, JointParameterIsEmpty) { - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", ""}); + this->SetUpController( + "test_gripper_action_position_controller_empty_joint", + controller_interface::return_type::ERROR); // configure failed, 'joints' is empty ASSERT_EQ( @@ -86,7 +90,9 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) { this->SetUpController(); - this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + rclcpp::spin_some(this->controller_->get_node()->get_node_base_interface()); // configure successful ASSERT_EQ( @@ -98,29 +104,14 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); ASSERT_THAT( cmd_if_conf.names, - UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + UnorderedElementsAre(std::string("joint1/") + hardware_interface::HW_IF_POSITION)); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = this->controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); - ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint1/position", "joint1/velocity")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); - - // activate failed, 'joint4' is not a valid joint name for the hardware - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - TEST_F(GripperControllerTest, ActivateSuccess) { this->SetUpController(); @@ -167,3 +158,12 @@ TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index 8bc6ab954c..d206097782 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -42,7 +42,8 @@ class GripperControllerTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpController(); + void SetUpController( + const std::string & controller_name, controller_interface::return_type expected_result); void SetUpHandles(); protected: From 2674f6d2d1821231781d9528089c7d9d24e76a33 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 14:09:30 +0200 Subject: [PATCH 521/524] Fix WaitSet issue in tests (#1206) --- .../test_ackermann_steering_controller.cpp | 2 +- .../test_ackermann_steering_controller.hpp | 49 +++++++--------- .../test/test_admittance_controller.cpp | 2 +- .../test/test_admittance_controller.hpp | 49 +++++++--------- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 48 ++++++++-------- .../test/test_diff_drive_controller.cpp | 15 ++--- .../test_joint_group_effort_controller.cpp | 23 +++----- .../test_joint_group_effort_controller.hpp | 2 + .../test_force_torque_sensor_broadcaster.cpp | 21 +++++-- .../test/test_forward_command_controller.cpp | 24 +++----- .../test/test_forward_command_controller.hpp | 2 + ...i_interface_forward_command_controller.cpp | 24 +++----- ...i_interface_forward_command_controller.hpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 20 +++++-- .../test/test_joint_state_broadcaster.cpp | 57 ++++++++++++------- .../test/test_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller_utils.hpp | 28 ++++----- pid_controller/test/test_pid_controller.cpp | 2 +- pid_controller/test/test_pid_controller.hpp | 52 ++++++++--------- .../test_joint_group_position_controller.cpp | 23 +++----- .../test_joint_group_position_controller.hpp | 2 + .../test/test_range_sensor_broadcaster.cpp | 20 +++++-- .../test_steering_controllers_library.hpp | 49 +++++++--------- .../test/test_tricycle_controller.cpp | 15 ++--- .../test_tricycle_steering_controller.cpp | 2 +- .../test_tricycle_steering_controller.hpp | 49 ++++++++-------- .../test_joint_group_velocity_controller.cpp | 23 +++----- .../test_joint_group_velocity_controller.hpp | 2 + 29 files changed, 294 insertions(+), 319 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 718e6f5856..de45accc0a 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -257,7 +257,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat EXPECT_EQ(msg.steering_angle_command[1], 4.4); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 363db793d5..9379d956c9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -76,14 +76,7 @@ class TestableAckermannSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -97,30 +90,25 @@ class TestableAckermannSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -214,36 +202,43 @@ class AckermannSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 6b03249df8..256fe1e5fe 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -277,7 +277,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); broadcast_tfs(); ASSERT_EQ( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 19908d7f9f..a464d050be 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -57,15 +57,6 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} - } // namespace // subclassing and friending so we can access member variables @@ -92,39 +83,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); - } - return ret; + return admittance_controller::AdmittanceController::on_configure(previous_state); } /** * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = - input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } private: - rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; - rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -279,9 +258,12 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + ControllerStateMsg::SharedPtr received_msg; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/status", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node_->get_node_base_interface()); // call update to publish the test value ASSERT_EQ( @@ -289,11 +271,18 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node_->get_clock()->now() + timeout; + while (!received_msg && test_subscription_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands() diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 9904f791b6..a06aabbd20 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -238,7 +238,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(0.1, 0.2); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index dcdb7ed169..4643ee3ee9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -74,13 +74,7 @@ class TestableBicycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -94,30 +88,25 @@ class TestableBicycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -187,34 +176,43 @@ class BicycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on max_sub_check_loop_count - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f3f99ac8d8..4976da4dfa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -57,22 +57,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } /** @@ -550,7 +545,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 200a1beda8..06bbc6fd22 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupEffortControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..e871ac6c43 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -24,6 +24,7 @@ #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -57,6 +58,7 @@ class JointGroupEffortControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2412361352..9528adbb0f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -75,31 +75,40 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg) { // create a new subscriber + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); + wrench_msg = *received_msg; } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index c31c8a964c..fcfa65ee1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -30,24 +30,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void ForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -71,6 +59,7 @@ void ForwardCommandControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) @@ -320,10 +309,13 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..2284d46d61 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -24,6 +24,7 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -69,6 +70,7 @@ class ForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7879d5c1d7..c52f4f3ab6 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -32,24 +32,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void MultiInterfaceForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -74,6 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params command_ifs.emplace_back(joint_1_vel_cmd_); command_ifs.emplace_back(joint_1_eff_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); if (set_params_and_activate) { @@ -273,10 +262,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..78b244847b 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -26,6 +26,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -77,6 +78,7 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 25a39a8b4d..e8aa571112 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -77,31 +77,39 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber + sensor_msgs::msg::Imu::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(imu_msg, msg_info)); + imu_msg = *received_msg; } TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 2faa55f467..4e4c0631f0 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -664,31 +664,41 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + sensor_msgs::msg::JointState::SharedPtr received_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); + joint_state_msg = *received_msg; } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -731,51 +741,58 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) + { dynamic_joint_state_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + dynamic_joint_state_msg.reset(); + ASSERT_FALSE(dynamic_joint_state_msg); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (dynamic_joint_state_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - - // take message from subscription - control_msgs::msg::DynamicJointState dynamic_joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); + ASSERT_TRUE(dynamic_joint_state_msg); const size_t NUM_JOINTS = 3; const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; - ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, + dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); const auto index_in_original_order = std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].values, + dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 611c1d4c1b..21c5705505 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -253,7 +253,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); + subscribeToState(executor); updateController(); // Spin to receive latest state @@ -644,7 +644,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun SetUpAndActivateTrajectoryController( executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); - subscribeToState(); + subscribeToState(executor); size_t n_joints = joint_names_.size(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 1750f7d30d..a15b2e5bc5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -94,11 +94,6 @@ class TestableJointTrajectoryController const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); - // this class can still be useful without the wait set - if (joint_command_subscriber_) - { - joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); - } return ret; } @@ -108,19 +103,17 @@ class TestableJointTrajectoryController * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout */ - bool wait_for_trajectory( + void wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } void set_joint_names(const std::vector & joint_names) @@ -223,7 +216,6 @@ class TestableJointTrajectoryController } } - rclcpp::WaitSet joint_cmd_sub_wait_set_; rclcpp::NodeOptions node_options_; }; @@ -399,7 +391,7 @@ class TrajectoryControllerTest : public ::testing::Test static void TearDownTestCase() { rclcpp::shutdown(); } - void subscribeToState() + void subscribeToState(rclcpp::Executor & executor) { auto traj_lifecycle_node = traj_controller_->get_node(); @@ -416,6 +408,14 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } } /// Publish trajectory msgs with multiple points diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a44347f5f1..4e9601ae66 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -495,7 +495,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1c356263e7..8c36dca60f 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -65,13 +65,7 @@ class TestablePidController : public pid_controller::PidController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = pid_controller::PidController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; + return pid_controller::PidController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -85,30 +79,25 @@ class TestablePidController : public pid_controller::PidController * @brief wait_for_command blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -182,36 +171,43 @@ class PidControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_pid_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands( diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 60bff556db..a4a288a8e4 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupPositionControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..c7d704db52 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -24,6 +24,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -58,6 +59,7 @@ class JointGroupPositionControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a23d5e3cde..ef9d5187a3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -66,31 +66,39 @@ controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broad void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) { // create a new subscriber + sensor_msgs::msg::Range::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_range_sensor_broadcaster/range", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(range_msg, msg_info)); + range_msg = *received_msg; } TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b6fe1770c1..ae7c41a09b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -80,14 +80,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -101,26 +94,24 @@ class TestableSteeringControllersLibrary * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } // implementing methods which are declared virtual in the steering_controllers_library.hpp @@ -139,9 +130,6 @@ class TestableSteeringControllersLibrary } bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -235,36 +223,43 @@ class SteeringControllersLibraryFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 4be1680980..901597a8bc 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -62,22 +62,17 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } }; @@ -326,7 +321,7 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 328f5e4d6a..ad7f80607f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -241,7 +241,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index b4d58a95c0..8a3ea33925 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -75,14 +75,7 @@ class TestableTricycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -96,30 +89,25 @@ class TestableTricycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + return wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -201,34 +189,43 @@ class TricycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index a99ffaeebf..f9aa666e30 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupVelocityControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..3078359028 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" using hardware_interface::CommandInterface; @@ -58,6 +59,7 @@ class JointGroupVelocityControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ From 96a8d5798c1e2a80b104d2fb3312e1444dc142c5 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 16 Jul 2024 06:50:22 -0600 Subject: [PATCH 522/524] Unused header cleanup (#1199) --- .../test/test_ackermann_steering_controller.cpp | 2 -- .../test/test_ackermann_steering_controller.hpp | 8 ++------ ...st_ackermann_steering_controller_preceeding.cpp | 2 -- .../admittance_controller.hpp | 8 -------- .../admittance_controller/admittance_rule.hpp | 13 +++---------- .../admittance_controller/admittance_rule_impl.hpp | 5 +++-- .../src/admittance_controller.cpp | 4 ---- .../test/test_admittance_controller.cpp | 2 -- .../test/test_admittance_controller.hpp | 7 +++---- .../test/test_asset_6d_robot_description.hpp | 2 -- .../test/test_bicycle_steering_controller.cpp | 2 -- .../test/test_bicycle_steering_controller.hpp | 8 ++------ ...test_bicycle_steering_controller_preceeding.cpp | 2 -- .../diff_drive_controller.hpp | 4 ---- .../test/test_diff_drive_controller.cpp | 2 -- .../joint_group_effort_controller.hpp | 3 --- .../src/joint_group_effort_controller.cpp | 1 - .../test/test_joint_group_effort_controller.cpp | 5 ----- .../test/test_joint_group_effort_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 3 --- .../test/test_force_torque_sensor_broadcaster.cpp | 5 ----- .../test/test_force_torque_sensor_broadcaster.hpp | 4 ++-- .../forward_command_controller.hpp | 2 -- .../forward_controllers_base.hpp | 1 - .../multi_interface_forward_command_controller.hpp | 2 -- .../src/forward_command_controller.cpp | 3 --- .../src/forward_controllers_base.cpp | 2 -- .../hardware_interface_adapter.hpp | 1 - .../test/test_gripper_controllers.cpp | 4 ---- .../test/test_gripper_controllers.hpp | 4 ++-- .../imu_sensor_broadcaster.hpp | 3 --- .../test/test_imu_sensor_broadcaster.cpp | 5 ----- .../test/test_imu_sensor_broadcaster.hpp | 4 ++-- .../joint_state_broadcaster.hpp | 2 -- .../src/joint_state_broadcaster.cpp | 7 +------ .../test/test_joint_state_broadcaster.cpp | 5 +---- .../test/test_joint_state_broadcaster.hpp | 4 ++-- .../joint_trajectory_controller.hpp | 2 -- .../joint_trajectory_controller/tolerances.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 3 --- joint_trajectory_controller/src/trajectory.cpp | 1 - joint_trajectory_controller/test/test_assets.hpp | 2 -- .../test/test_load_joint_trajectory_controller.cpp | 4 ++-- .../test/test_tolerances.cpp | 5 ++--- .../test/test_trajectory.cpp | 7 ++----- .../test/test_trajectory_actions.cpp | 8 -------- .../test/test_trajectory_controller.cpp | 14 +------------- .../include/pid_controller/pid_controller.hpp | 6 ------ pid_controller/src/pid_controller.cpp | 3 --- pid_controller/test/test_pid_controller.cpp | 1 - pid_controller/test/test_pid_controller.hpp | 7 ++----- .../test/test_pid_controller_preceding.cpp | 2 -- .../joint_group_position_controller.hpp | 3 --- .../src/joint_group_position_controller.cpp | 1 - .../test/test_joint_group_position_controller.cpp | 5 ----- .../test/test_joint_group_position_controller.hpp | 4 ++-- .../range_sensor_broadcaster.hpp | 3 --- .../steering_controllers_library.hpp | 6 ------ .../steering_odometry.hpp | 3 +-- .../src/steering_controllers_library.cpp | 4 ---- .../src/steering_odometry.cpp | 1 - .../test/test_steering_controllers_library.cpp | 3 --- .../test/test_steering_controllers_library.hpp | 8 ++------ .../test/test_steering_odometry.cpp | 2 +- .../include/tricycle_controller/odometry.hpp | 2 +- .../tricycle_controller/tricycle_controller.hpp | 3 --- .../test/test_tricycle_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.hpp | 8 ++------ ...est_tricycle_steering_controller_preceeding.cpp | 2 -- .../joint_group_velocity_controller.hpp | 3 --- .../src/joint_group_velocity_controller.cpp | 1 - .../test/test_joint_group_velocity_controller.cpp | 5 ----- .../test/test_joint_group_velocity_controller.hpp | 4 ++-- 74 files changed, 45 insertions(+), 243 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index de45accc0a..c04e87be95 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 9379d956c9..320a1a91a6 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "ackermann_steering_controller/ackermann_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 1a16bed838..96dd20d80e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 6ff6cdae7a..9be6c3298c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,7 +17,6 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include #include #include #include @@ -29,21 +28,14 @@ #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" -#include "trajectory_msgs/msg/joint_trajectory.hpp" - namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 36c027491c..7223dbe9d1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -18,23 +18,16 @@ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #include -#include -#include + #include #include #include +#include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" -#include "control_toolbox/filters.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "controller_interface/controller_interface_base.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" -#include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_kdl/tf2_kdl.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9b03924882..77f1277b35 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,9 +22,10 @@ #include #include +#include +#include + #include "rclcpp/duration.hpp" -#include "rclcpp/utilities.hpp" -#include "tf2_ros/transform_listener.h" namespace admittance_controller { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..6e29b574a2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,17 +16,13 @@ #include "admittance_controller/admittance_controller.hpp" -#include #include -#include #include #include #include #include "admittance_controller/admittance_rule_impl.hpp" #include "geometry_msgs/msg/wrench.hpp" -#include "rcutils/logging_macros.h" -#include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 256fe1e5fe..5c4bbe9438 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -16,9 +16,7 @@ #include "test_admittance_controller.hpp" -#include #include -#include #include // Test on_init returns ERROR when a required parameter is missing diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index a464d050be..d2aef3df4b 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -17,6 +17,8 @@ #ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ #define TEST_ADMITTANCE_CONTROLLER_HPP_ +#include + #include #include #include @@ -25,21 +27,18 @@ #include #include -#include "gmock/gmock.h" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/parameter_value.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" #include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" -#include "trajectory_msgs/msg/joint_trajectory.hpp" // TODO(anyone): replace the state and command message types using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; diff --git a/admittance_controller/test/test_asset_6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp index 4d38df7c30..11412ebbab 100644 --- a/admittance_controller/test/test_asset_6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ -#include - namespace ros2_control_test_assets { const auto valid_6d_robot_urdf = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index a06aabbd20..573992b24e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 4643ee3ee9..a6b3f8ae40 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "bicycle_steering_controller/bicycle_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index bc3a182753..0bc03f4886 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 72b38f7d2d..ac34b81eca 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -30,15 +30,11 @@ #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4976da4dfa..bc664f0711 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -26,7 +25,6 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 562bbea52f..b07e8a630c 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -15,11 +15,8 @@ #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#include - #include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace effort_controllers { diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index c3748d493c..e7d0f274a5 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace effort_controllers diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 06bbc6fd22..c19cbff9cf 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index e871ac6c43..5af880d792 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 754d1de9ba..bd477ed68a 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,15 +20,12 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 9528adbb0f..2872e8a60f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -22,14 +22,9 @@ #include #include -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; using testing::IsEmpty; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe5b0ab3ba..fe7fb0c174 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 91e3aae480..12f2a28b22 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index d60245f328..4858e5ab64 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,6 @@ #include "controller_interface/controller_interface.hpp" #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "std_msgs/msg/float64_multi_array.hpp" diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index fd7c0d480e..50476c62e3 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0305a37e4e..78fe8c9191 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,14 +14,11 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include #include #include -#include #include #include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..331d3f9eea 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,10 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" -#include #include #include -#include #include #include "controller_interface/helpers.hpp" diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index b125ab12d0..2cc24794b8 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 9f7e024917..97fef24eaf 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -23,9 +22,6 @@ #include "test_gripper_controllers.hpp" #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 4983c8102d..cb92bc9dcd 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -15,12 +15,12 @@ #ifndef TEST_GRIPPER_CONTROLLERS_HPP_ #define TEST_GRIPPER_CONTROLLERS_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "gripper_controllers/gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index cd2a44d32b..1ba0b032ac 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -20,14 +20,11 @@ #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/imu_sensor.hpp" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index e8aa571112..a2da6713bb 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,12 +23,7 @@ #include #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..0f3286c302 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_ #define TEST_IMU_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index f1c532dce9..b3fa69f94c 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,8 +25,6 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a53fe2b3c4..5eb0a9b9b6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -14,21 +14,16 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" -#include +#include #include #include #include #include #include -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcpputils/split.hpp" -#include "rcutils/logging_macros.h" #include "std_msgs/msg/header.hpp" namespace rclcpp_lifecycle diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 4e4c0631f0..e479f344bf 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,12 +23,9 @@ #include "gmock/gmock.h" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fa9d29c936..3e54116d5c 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_STATE_BROADCASTER_HPP_ #define TEST_JOINT_STATE_BROADCASTER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index c3fbb58456..e22dee8946 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -15,7 +15,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ -#include #include // for std::reference_wrapper #include #include @@ -36,7 +35,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c4404920df..8c556703ab 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -38,9 +38,6 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/time.hpp" - namespace joint_trajectory_controller { /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..ddcf57f508 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -22,10 +22,7 @@ #include #include "angles/angles.h" -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "controller_interface/helpers.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..54f785a070 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -20,7 +20,6 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp index ccdbaf4c8a..df6f2a37c7 100644 --- a/joint_trajectory_controller/test/test_assets.hpp +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSETS_HPP_ #define TEST_ASSETS_HPP_ -#include - namespace test_trajectory_controllers { const auto urdf_rrrbot_revolute = diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 5b36afc4e8..8d0fbb3e75 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -#include "gmock/gmock.h" +#include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index bd6304df29..106a9a761f 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include -#include #include -#include "gmock/gmock.h" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6e0c53ac77..b0d7ad8e18 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include #include #include -#include "gmock/gmock.h" - -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3e65016403..164f7d0e11 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -15,38 +15,30 @@ #ifndef _MSC_VER #include #endif -#include #include #include #include #include -#include #include #include -#include #include #include #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" -#include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" -#include "hardware_interface/resource_manager.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "test_trajectory_controller_utils.hpp" using std::placeholders::_1; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21c5705505..e83e74ef41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -12,35 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -#include #include -#include -#include #include -#include #include #include #include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f7b8cc4491..236b067929 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -28,17 +28,11 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "control_msgs/msg/joint_controller_state.hpp" - -#include "control_msgs/msg/pid_state.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - namespace pid_controller { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b76926d5a0..c8e6cc0fe0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -24,9 +24,6 @@ #include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" -#include "controller_interface/helpers.hpp" - -#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" namespace diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 4e9601ae66..9b53dccb23 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 8c36dca60f..895c56c1a5 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -18,20 +18,17 @@ #ifndef TEST_PID_CONTROLLER_HPP_ #define TEST_PID_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "pid_controller/pid_controller.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 3e17e69286..498ca633da 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -17,10 +17,8 @@ #include "test_pid_controller.hpp" -#include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 47705b6a3d..4eaf9086e4 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -15,11 +15,8 @@ #ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" #include "position_controllers/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace position_controllers { diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 8335a200c4..1074ba7ef7 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace position_controllers diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a4a288a8e4..96a5cead17 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index c7d704db52..fefd2073e3 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index b2e5fbfac0..5a93f95982 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -20,13 +20,10 @@ #define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/range_sensor.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e417b9fcd1..84a892d79e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -15,21 +15,16 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ -#include #include #include -#include #include -#include #include #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/set_bool.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" @@ -37,7 +32,6 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index dadc5730d4..5b67797b79 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -22,8 +22,7 @@ #include #include -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 1aef556212..f193098f82 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -16,15 +16,11 @@ #include #include -#include #include #include #include -#include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index f8b42771e2..ba431faf33 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -20,7 +20,6 @@ #include "steering_controllers_library/steering_odometry.hpp" #include -#include #include namespace steering_odometry diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 98ab97fdc3..fca7d00946 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -17,11 +17,8 @@ #include #include #include -#include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" - class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index ae7c41a09b..b257562dcd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -15,21 +15,17 @@ #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d93e29eca5..e3c8db6c15 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#include #include "steering_controllers_library/steering_odometry.hpp" diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 13d65a980a..f3252df12f 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -21,7 +21,7 @@ #include -#include "rclcpp/time.hpp" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index b6f9aae417..010a890f52 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -31,12 +31,9 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 901597a8bc..e56e3afacb 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -29,7 +28,6 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" #include "tricycle_controller/tricycle_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index ad7f80607f..3f2589cb6c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 8a3ea33925..184439aa6f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -15,21 +15,17 @@ #ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 6f2913aeb8..2170659ee7 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index b5c36f433a..e443ba76cc 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -15,10 +15,7 @@ #ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "velocity_controllers/visibility_control.h" namespace velocity_controllers diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index eb55e052dc..97f6713d72 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -16,7 +16,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index f9aa666e30..d781b6ca5c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 3078359028..0ebe68833c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" From f7b1af1841c42bf6c1dc617b170e5abc0150c99c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 11:40:05 +0200 Subject: [PATCH 523/524] Change the subscription timeout in the tests to 5ms (#1219) --- .../test/test_ackermann_steering_controller.hpp | 2 +- admittance_controller/test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_force_torque_sensor_broadcaster.cpp | 2 +- imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 +- joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 4 ++-- pid_controller/test/test_pid_controller.hpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 320a1a91a6..b0b0944de2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -216,7 +216,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d2aef3df4b..b2a95c12fa 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -270,7 +270,7 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node_->get_clock()->now() + timeout; while (!received_msg && test_subscription_node_->get_clock()->now() < until) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index a6b3f8ae40..bfcf9fa424 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -190,7 +190,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2872e8a60f..1acddcb64a 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -85,7 +85,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index a2da6713bb..132bf4006e 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -86,7 +86,7 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index e479f344bf..71e65a978d 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -677,7 +677,7 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (!received_msg && test_node.get_clock()->now() < until) { @@ -755,7 +755,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (test_node.get_clock()->now() < until) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 895c56c1a5..326393bac0 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -186,7 +186,7 @@ class PidControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ef9d5187a3..e8be5e3d0a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -80,7 +80,7 @@ void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Ran while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b257562dcd..8c4955b7eb 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -237,7 +237,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 184439aa6f..0fbea25abf 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -203,7 +203,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { From 990bfb645425c166a923ddb53ddd98ec66ea42d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 18 Jul 2024 14:47:05 +0200 Subject: [PATCH 524/524] Remove duplicated call to rclcpp::shutdown in test (#1220) --- diff_drive_controller/test/test_load_diff_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index bcd334631f..aa23f83ec9 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -41,6 +41,5 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); int result = RUN_ALL_TESTS(); rclcpp::shutdown(); - rclcpp::shutdown(); return result; }