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-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index d8f77a323a..174f5efcd3 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -26,6 +26,7 @@ jobs: joint_trajectory_controller gripper_controllers position_controllers + range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes tricycle_controller @@ -56,6 +57,7 @@ jobs: joint_trajectory_controller gripper_controllers position_controllers + range_sensor_broadcaster ros2_controllers ros2_controllers_test_nodes tricycle_controller diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 2ad549b0f8..1dc51cca27 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -37,7 +37,26 @@ jobs: target-ros2-distro: ${{ inputs.ros_distro }} # 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/.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 diff --git a/README.md b/README.md index 6a59d0054e..d8091e0b63 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,9 @@ # ros2_controllers -Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Navigation2. +[![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/branch/humble/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 Nav2. ## Build status diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 27bf6cdc8f..9094e77913 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- 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/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index a015873b99..f6d00dc8fd 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 2.23.0 + 2.29.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 6e8b5ae9ca..25d8f756cb 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* Update docs for diff drive controller (`#751 `_) (`#753 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- +* Activate AdmittanceControllerTestParameterizedInvalidParameters (`#711 `_) (`#733 `_) +* Fix file name for include guard (backport `#681 `_) +* Fix out of bound access in admittance controller (`#721 `_) (`#722 `_) +* Contributors: Christoph Fröhlich + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) 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/admittance_controller/package.xml b/admittance_controller/package.xml index 6d51b0e75c..67b8da731d 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.23.0 + 2.29.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..7aa099db5a 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -67,10 +67,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 be78f05bb9..04c629a21b 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" @@ -454,9 +454,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); } }; 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_ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 70632904ae..f051455010 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- 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/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 59b24b248a..be0011cc5e 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 2.23.0 + 2.29.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/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 diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 7525d4998d..4a2815d6e9 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) (`#823 `_) +* Contributors: Tony Baltovski + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) (`#777 `_) +* Update docs for diff drive controller (`#751 `_) (`#753 `_) +* Contributors: Christoph Fröhlich, Jules CARPENTIER + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) 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/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8e0ec4c9d2..d09bb640dd 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.23.0 + 2.29.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1c88d2085c..c8032e5dc5 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_; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 5d20970cab..e878ad5481 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, diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 763381c065..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,33 +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: - 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> diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 4d8c3752ae..5d8797628e 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -27,17 +27,19 @@ 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. 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) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 8cfcd76934..38a60737e5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) 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/effort_controllers/package.xml b/effort_controllers/package.xml index 0a32f9cc7c..ec801db332 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.23.0 + 2.29.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 e92b4cd215..6e4942f013 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- +* [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (backport `#698 `_) (`#750 `_) + * [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 + (cherry picked from commit 32aaef7552638826aba0b3f3a72b1c1453739afa) + * Fix "parameter is already declared" error + --------- + Co-authored-by: Noel Jiménez García + Co-authored-by: Christoph Froehlich +* Contributors: mergify[bot] + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Broadcaster parameters (`#650 `_) (`#678 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 297a7d2812..7c35b17cb7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.23.0 + 2.29.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl 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/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index dc9c2db8be..2bec94c307 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- +* Rearrange controllers overview page (`#846 `_) (`#847 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- +* Use tabs (`#743 `_) (`#746 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index ee0af57d7b..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` @@ -16,17 +16,27 @@ 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. -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 diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 821103dfda..24a349f2db 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.23.0 + 2.29.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index da64f0583a..b3d5b93a60 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- +* Add test for effort gripper controller (`#769 `_) (`#867 `_) +* Contributors: mergify[bot] + +2.28.0 (2023-11-30) +------------------- +* Fixed implementation so that effort_controllers/GripperActionController works. (`#756 `_) (`#868 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 89bf232fad..cf3ec193f3 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -124,7 +124,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); diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a63e5097f2..2730d3287f 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.23.0 + 2.29.0 The gripper_controllers package Bence Magyar 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 diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index fd9449ea0e..2b2a86d60d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Broadcaster parameters (`#650 `_) (`#678 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index df16e434cb..6b079c70d7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.23.0 + 2.29.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 145b4a50da..1f163cd33c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- +* joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) (`#863 `_) +* Contributors: mergify[bot] + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1fd7c889e9..6263f3bce6 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.23.0 + 2.29.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 7f95e232e2..98dac68924 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"}; diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 3163f94965..cea342497b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* [JTC] Rename parameter: normalize_error to angle_wraparound (`#772 `_) (`#776 `_) +* Remove wrong description (`#742 `_) (`#747 `_) +* [JTC] Update trajectory documentation (`#714 `_) (`#741 `_) +* Contributors: Christoph Fröhlich + +2.24.0 (2023-08-07) +------------------- +* [JTC] Disable use of closed-loop PID adapter if controller is used in open-loop mode. (`#551 `_) (`#740 `_) +* [JTC] Reject messages with effort fields (`#699 `_) (`#719 `_) (`#738 `_) +* [JTC] Re-enabling test, bugfixing and hardening. Adding a parameter to define when trajectories with non-zero velocity at the end are used. (backport `#705 `_) (`#706 `_) +* Small improvement in remapping (`#393 `_) (`#724 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Bence Magyar + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index cf64269e65..73d01ef132 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -78,13 +78,13 @@ 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 + ) + set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300) + target_link_libraries(test_trajectory_actions + joint_trajectory_controller + ) endif() diff --git a/joint_trajectory_controller/doc/new_trajectory.png b/joint_trajectory_controller/doc/new_trajectory.png new file mode 100644 index 0000000000..b7c07eeffe Binary files /dev/null and b/joint_trajectory_controller/doc/new_trajectory.png differ diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst new file mode 100644 index 0000000000..f62320870c --- /dev/null +++ b/joint_trajectory_controller/doc/parameters.rst @@ -0,0 +1,139 @@ +: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. + +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). + + 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 ``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 diff --git a/joint_trajectory_controller/doc/spline_acceleration.png b/joint_trajectory_controller/doc/spline_acceleration.png new file mode 100644 index 0000000000..98672beedd Binary files /dev/null and b/joint_trajectory_controller/doc/spline_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position.png b/joint_trajectory_controller/doc/spline_position.png new file mode 100644 index 0000000000..79167f8500 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity.png b/joint_trajectory_controller/doc/spline_position_velocity.png new file mode 100644 index 0000000000..0e959295fb Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity.png differ 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 0000000000..70152eec4e Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png new file mode 100644 index 0000000000..a0f06e33c5 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png new file mode 100644 index 0000000000..a1e29de3fc Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png differ diff --git a/joint_trajectory_controller/doc/spline_velocity.png b/joint_trajectory_controller/doc/spline_velocity.png new file mode 100644 index 0000000000..d427f4da88 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_velocity.png differ 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 0000000000..f73310c031 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_wrong_points.png differ diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst new file mode 100644 index 0000000000..44f385de27 --- /dev/null +++ b/joint_trajectory_controller/doc/trajectory.rst @@ -0,0 +1,168 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/trajectory.rst + +.. _joint_trajectory_controller_trajectory_representation: + +Trajectory Representation +--------------------------------- + +Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. + +Currently, two interpolation methods are implemented: ``none`` and ``spline``. +By default, a spline interpolator is provided, but it's possible to support other representations. + +.. warning:: + The user has to ensure that the correct inputs are provided for the trajectory, which are needed + by the controller's setup of command interfaces and PID configuration. There is no sanity check and + missing fields in the sampled trajectory might cause segmentation faults. + +Interpolation Method ``none`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +It returns the initial point until the time for the first trajectory data point is reached. Then, it simply takes the next given datapoint. + +.. warning:: + It does not deduce (integrate) trajectory from derivatives, nor does it calculate derivatives. + I.e., one has to provide position and its derivatives as needed. + +Interpolation Method ``spline`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The spline interpolator uses the following interpolation strategies depending on the waypoint specification: + +* Linear: + + * Used, if only position is specified. + * Returns position and velocity + * Guarantees continuity at the position level. + * Discouraged because it yields trajectories with discontinuous velocities at the waypoints. + +* Cubic: + + * Used, if position and velocity are specified. + * Returns position, velocity, and acceleration. + * Guarantees continuity at the velocity level. + +* Quintic: + + * Used, if position, velocity and acceleration are specified + * Returns position, velocity, and acceleration. + * Guarantees continuity at the acceleration level. + +Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method. + +Visualized Examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms. + +* Sampled trajectory with linear spline if position is given only: + +.. image:: spline_position.png + :alt: Sampled trajectory with splines if position is given only + +* Sampled trajectory with cubic splines if velocity is given only (no deduction for interpolation method ``none``): + +.. image:: spline_velocity.png + :alt: Sampled trajectory with splines if velocity is given only + +* Sampled trajectory if position and velocity is given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where velocity only was given as input. + +.. image:: spline_position_velocity.png + :alt: Sampled trajectory if position and velocity is given + +* Sampled trajectory with quintic splines if acceleration is given only (no deduction for interpolation method ``none``): + +.. image:: spline_acceleration.png + :alt: Sampled trajectory with splines if acceleration is given only + +* Sampled trajectory if position, velocity, and acceleration points are given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where acceleration only was given as input. + +.. image:: spline_position_velocity_acceleration.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given, but with a nonzero initial point: + +.. image:: spline_position_velocity_acceleration_initialpoint.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given but with the first point starting at ``t=0``: + +.. note:: + If the first point is starting at ``t=0``, there is no interpolation from the initial point to the trajectory. + +.. image:: spline_position_velocity_acceleration_initialpoint_notime.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point and first point starting at ``t=0`` + +* Sampled trajectory with splines if inconsistent position, velocity, and acceleration points are given: + +.. note:: + Interpolation method ``none`` only gives the next input points, while the ``spline`` interpolation method shows high overshoot to match the given trajectory points. + +.. image:: spline_wrong_points.png + :alt: Sampled trajectory with splines if inconsistent position, velocity, and acceleration is given + +.. _joint_trajectory_controller_trajectory_replacement: + +Trajectory Replacement +--------------------------------- +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. [#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 0000000000..582a6acce8 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_future.png differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_now.png b/joint_trajectory_controller/doc/trajectory_replacement_now.png new file mode 100644 index 0000000000..a7cd2f6bc7 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_now.png differ 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 0000000000..a719f6fc66 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_past.png differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 798cc161e5..92f0f9629d 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -12,21 +12,7 @@ Trajectories are specified as a set of waypoints to be reached at specific time which the controller attempts to execute as well as the mechanism allows. 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]_ [#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]_ ------------------------------- @@ -115,116 +101,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. - -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). - - Default: 20.0 - -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 (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) - 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 - -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: @@ -257,8 +137,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: @@ -301,17 +180,16 @@ Services Query controller state at any future time -Specialized versions of JointTrajectoryController (TBD in ...) +Further information -------------------------------------------------------------- -The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). - -The following version of the Joint Trajectory Controller are available mapping the following interfaces: +.. toctree:: + :titlesonly: -* position_controllers::JointTrajectoryController + Trajectory Representation + joint_trajectory_controller Parameters .. rubric:: Footnote .. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ -.. [#f2] Adolfo Rodriguez: `Understanding trajectory replacement `_ 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 91c652013e..6c909e3278 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 @@ -19,10 +19,11 @@ #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) @@ -94,6 +95,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/package.xml b/joint_trajectory_controller/package.xml index 17a97945b8..8a0c9840dd 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.23.0 + 2.29.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 602b5528ff..cff2918b75 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -67,6 +67,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; } @@ -136,7 +145,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]; } @@ -234,7 +245,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) { @@ -281,14 +292,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 @@ -683,7 +687,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_) @@ -717,12 +722,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()) @@ -1312,8 +1326,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]; @@ -1349,8 +1367,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; @@ -1412,6 +1431,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) { @@ -1452,11 +1485,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/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index e710b33d07..bb0b45bb80 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: { @@ -74,6 +74,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: { @@ -104,7 +109,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: { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 10dd3a6b90..fdea551c30 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -66,21 +66,20 @@ 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; - executor_ = std::make_unique(); - - SetUpAndActivateTrajectoryController(true, parameters); - - executor_->add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpAndActivateTrajectoryController( + executor_, true, parameters, separate_cmd_and_state_values); 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 +131,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest if (setup_executor_) { setup_executor_ = false; - executor_->cancel(); + executor_.cancel(); executor_future_handle_.wait(); } } @@ -169,7 +168,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; @@ -201,7 +200,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(); @@ -227,12 +243,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(); @@ -274,11 +293,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 @@ -312,11 +338,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 @@ -367,12 +400,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; @@ -417,14 +453,17 @@ 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); - 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; @@ -436,15 +475,13 @@ TEST_F(TestTrajectoryActions, 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; @@ -466,15 +503,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail) 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)); + // run an update, it should be holding the last received goal + 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(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); + } } -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; @@ -514,14 +554,17 @@ 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); - 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(); @@ -563,9 +606,180 @@ 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)); + + 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]); + } +} + +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()); - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); + 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, + ::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 e3512d4d94..72cd2e8ca9 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; @@ -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); @@ -199,123 +201,12 @@ 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; - 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); @@ -338,13 +229,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - // update for 0.25 seconds - 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(); } @@ -369,6 +253,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(); @@ -428,21 +314,15 @@ 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()); - - // 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)); - - 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); + // reactivated + // wait so controller process the third point when reactivated + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + ActivateTrajectoryController(); + state = traj_controller_->get_state(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // state = traj_controller_->get_node()->configure(); - // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + // TODO(christophfroehlich) add test if there is no active trajectory after + // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController) executor.cancel(); } @@ -605,7 +485,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(); @@ -659,14 +541,20 @@ 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], 3 * 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()) @@ -688,9 +576,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(); @@ -703,7 +594,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(); @@ -762,14 +655,9 @@ 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]); - // 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->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], @@ -778,6 +666,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); @@ -785,16 +674,39 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) else { // interpolated points_velocities only - EXPECT_LE(0.0, joint_vel_[2]); + // 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_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(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(); @@ -847,804 +759,880 @@ TEST_P(TrajectoryControllerTestParameterized, test_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) -// { -// 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 check if use_closed_loop_pid is active + */ +TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) +{ + rclcpp::executors::MultiThreadedExecutor executor; -// 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]); -// } + SetUpAndActivateTrajectoryController(executor); -// 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]); -// } -// } + 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 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); +/** + * @brief check if velocity error is calculated correctly + */ +TEST_P(TrajectoryControllerTestParameterized, velocity_error) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {}, true); + subscribeToState(); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + size_t n_joints = joint_names_.size(); -// 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; + // 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); -// { -// 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(); -// } + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); -// /** -// * @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); + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + // 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()); + } -// 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; + // 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]); + } -// { -// 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(); -// } + 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( -// 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)); -// } +/** + * @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; + } + trajectory_publisher_->publish(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)); -// } + 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)); -// /** -// * @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(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_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()) + { + 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 +} -// 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); -// } +/** + * @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); -// 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); -// } + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); -// 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(); -// } + 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; -// 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(); -// } + { + 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); + } -// // 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); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration::from_seconds(0.25)); -// // 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); + double threshold = 0.001; -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + 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"; + } -// 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(); -// } + 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"; + } -// // 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_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 -// // 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); + executor.cancel(); +} -// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +/** + * @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); -// 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(); -// } + 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]; + 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; + + 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"; + } + + 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"; + } + + 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"; + } + + 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)); + + // Effort is not supported in trajectory message + traj_msg = good_traj_msg; + 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 + 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(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); +} + +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.}}}; + 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()); + 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(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); +} + +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(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]}; + 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( @@ -1684,26 +1672,25 @@ 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"})))); // 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 0c01eb1357..874a814a5d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -105,15 +105,24 @@ 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_; } 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_; } + bool is_open_loop() { return params_.open_loop_control; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -316,13 +325,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 = {}) { @@ -342,14 +352,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; @@ -357,14 +368,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; } @@ -386,9 +397,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; } } @@ -413,7 +428,7 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties 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); } } @@ -423,7 +438,8 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties 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); } } } diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 37d356d773..7aec5c01b3 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) 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/position_controllers/package.xml b/position_controllers/package.xml index cd120bb7ba..870ab7639f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.23.0 + 2.29.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..a7edb84aed --- /dev/null +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -0,0 +1,152 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package range_sensor_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* add a broadcaster for range sensor (backport `#725 `_) (`#766 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- + +2.22.0 (2023-06-14) +------------------- + +2.21.0 (2023-05-28) +------------------- + +2.20.0 (2023-05-14) +------------------- + +2.19.0 (2023-05-02) +------------------- + +2.18.0 (2023-04-29) +------------------- + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.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/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..3c945505d9 --- /dev/null +++ b/range_sensor_broadcaster/package.xml @@ -0,0 +1,31 @@ + + + + range_sensor_broadcaster + 2.29.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..72fdac3a5e --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -0,0 +1,137 @@ +// 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_->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..c869cc27f2 --- /dev/null +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster_parameters.yaml @@ -0,0 +1,15 @@ +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]",} 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..7dae66a611 --- /dev/null +++ b/range_sensor_broadcaster/test/range_sensor_broadcaster_params.yaml @@ -0,0 +1,11 @@ +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 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..02d7de814e --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -0,0 +1,247 @@ +// 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_)); +} + +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_)); + + 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_)); +} + +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_)); + + 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_)); +} + +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..2b38cfa53c --- /dev/null +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -0,0 +1,58 @@ +// 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; + + 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/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index f702291963..17ce2aab02 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* add a broadcaster for range sensor (backport `#725 `_) (`#766 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index f114c05264..70f4253a40 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.23.0 + 2.29.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios @@ -21,6 +21,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + range_sensor_broadcaster steering_controllers_library tricycle_controller tricycle_steering_controller diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 68e9e923de..c45c4188bf 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f02d0b5a17..71b5ff70b4 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.23.0 + 2.29.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 154a4ca266..96b986fc43 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.23.0", + version="2.29.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 d92f3beab5..aeacc048f8 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 033a32aa68..ea28002de9 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.23.0 + 2.29.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 7276cd2cd6..6e1874ee06 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.23.0", + version="2.29.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 6e5ab30bfb..eca3aa3110 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Steering controllers library: fix open loop mode (`#793 `_) (`#800 `_) +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- 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 00516b716f..25d0235818 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/package.xml b/steering_controllers_library/package.xml index 22a22be78c..3e40903573 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 2.23.0 + 2.29.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 179fc39869..26207d54b7 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -448,10 +448,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_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/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 98a4f656f4..e8c3965b3f 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -212,7 +212,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; diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 4221af6f47..3629cb7ab7 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index b9d5eeea10..a56dedd0af 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.23.0 + 2.29.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 50f00fdf4f..c45dac4bac 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* Improve docs (`#785 `_) (`#786 `_) +* Contributors: mergify[bot] + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- 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 diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 5aa18c46fc..e60c736b56 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 2.23.0 + 2.29.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 e916e8d151..eadc0db3cd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- + +2.26.0 (2023-10-03) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) (`#778 `_) +* Contributors: mergify[bot] + +2.25.0 (2023-09-15) +------------------- + +2.24.0 (2023-08-07) +------------------- + 2.23.0 (2023-06-23) ------------------- * Renovate load controller tests (`#569 `_) (`#677 `_) 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 diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index c5d16ffbcc..eff54c00ea 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.23.0 + 2.29.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios