diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index cc5dd32596..d2dba1bce8 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -3,6 +3,9 @@ on: workflow_dispatch: branches: - master + push: + branches: + - master pull_request: branches: - master @@ -42,7 +45,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index c868650adb..5d801016f9 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.0 + - uses: actions/setup-python@v4.7.1 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 5a01d23533..167386ceff 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -44,7 +44,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 723ca88779..d576811bdb 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- 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 40a3545b99..f866089f1c 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 3.15.0 + 3.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 593fa56562..e17d56865d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * Update docs for diff drive controller (`#751 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9f2de80dad..43a7ab0aad 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.15.0 + 3.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index be6ac52313..7c652e7fa7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- 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 ddc110f81a..8707502915 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 3.15.0 + 3.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 52c8572739..d670ccc047 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * Update docs for diff drive controller (`#751 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index cc849d12ef..611dfbe15a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.15.0 + 3.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0475f8f4fd..501c231def 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -34,10 +34,12 @@ The following is a step-by-step guide to create source files, basic tests, and c 3. Define a unique namespace for your controller. This is usually a package name written in ``snake_case``. 4. Define the class of the controller, extending ``ControllerInterface``, e.g., + .. code:: c++ - class ControllerName : public controller_interface::ControllerInterface - 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. + class ControllerName : public controller_interface::ControllerInterface + + 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. @@ -48,9 +50,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Include the header file of your controller and add a namespace definition to simplify further development. 2. (optional) Implement a constructor if needed. There, you could initialize member variables. - This could also be done in the ``init`` method. + This could also be done in the ``on_init`` method. - 3. Implement the ``init`` method. The first line usually calls the parent ``init`` method. + 3. Implement the ``on_init`` method. The first line usually calls the parent ``on_init`` method. Here is the best place to initialize the variables, reserve memory, and most importantly, declare node parameters used by the controller. If everything works fine return ``controller_interface::return_type::OK`` or ``controller_interface::return_type::ERROR`` otherwise. 4. Write the ``on_configure`` method. Parameters are usually read here, and everything is prepared so that the controller can be started. @@ -105,6 +107,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Add ament dependencies needed by the library. You should add at least those listed under 1. 5. Export for pluginlib description file using the following command: + .. code:: cmake pluginlib_export_plugin_description_file(controller_interface .xml) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4b93f1f69a..efc583475d 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- 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 d0aca9701a..9e3cc8bc32 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 296cad54bf..70fe56b159 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * [ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4cbad1c014..12ec3d1945 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b3d33964b6..ca209a52f5 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index a91479c9af..01aa2492a8 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -16,8 +16,18 @@ Hardware interface type This controller can be used for every type of command interface. + +ROS 2 interface of the controller +--------------------------------- + +Topics +^^^^^^^ + +~/commands (input topic) [std_msgs::msg::Float64MultiArray] + Target joint commands + Parameters ------------- +^^^^^^^^^^^^^^ This controller uses the `generate_parameter_library `_ to handle its parameters. diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index af90230c7e..c8d0b99a59 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 82cf6cf45b..bc66422a37 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * Add test for effort gripper controller (`#769 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d3c8d9bf9f..dcd778363e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.15.0 + 3.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 915544197c..477134c496 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 31684ec958..6173569630 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 57bc1b7f45..35298737f0 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 248dbcb099..05f761169a 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.15.0 + 3.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8b1a1c6a54..a322fef5ad 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- +* [Docs] Improve interface description of JTC (`#770 `_) +* [JTC] Add time-out for trajectory interfaces (`#609 `_) +* [JTC] Rename parameter: normalize_error to angle_wraparound (`#772 `_) +* [JTC] Fix hold position mode with goal_time>0 (`#758 `_) +* [JTC] Add note on goal_time=0 in docs (`#773 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- * [JTC] Make most parameters read-only (`#771 `_) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index b9d8e274cf..deedc4a7b6 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -14,16 +14,24 @@ Waypoints consist of positions, and optionally velocities and accelerations. *Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ -Hardware interface type [#f1]_ +Hardware interface types ------------------------------- -Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time: +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations: + +* ``position`` +* ``position``, ``velocity`` +* ``position``, ``velocity``, ``acceleration`` +* ``velocity`` +* ``effort`` + +This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time: * For command interfaces ``position``, the desired positions are simply forwarded to the joints, * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. -* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`). +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`). -This leads to the the following allowed combinations of command and state interfaces: +This leads to the following allowed combinations of command and state interfaces: * With command interface ``position``, there are no restrictions for state interfaces. * With command interface ``velocity``: @@ -32,12 +40,9 @@ This leads to the the following allowed combinations of command and state interf * no restrictions otherwise. * With command interface ``effort``, state interfaces must include ``position, velocity``. -* With command interface ``acceleration``, there are no restrictions for state interfaces. Example controller configurations can be found :ref:`below `. -Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). - Other features -------------- @@ -118,14 +123,8 @@ States ,,,,,,,,,,,,,,,,,, The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. - -Legal combinations of state interfaces are: -* ``position`` -* ``position`` and ``velocity`` -* ``position``, ``velocity`` and ``acceleration`` -* ``effort`` +Legal combinations of state interfaces are given in section `Hardware Interface Types`_. Commands ,,,,,,,,, diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 521aa0ea9d..d8f2e8e997 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.15.0 + 3.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index bb387dbc4d..07d60ce077 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -57,8 +58,6 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } - void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) @@ -911,7 +910,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { - // don't check against a value, because spline intepolation might overshoot depending on + // don't check against a value, because spline interpolation might overshoot depending on // interface combinations EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); @@ -929,50 +928,82 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor); + std::vector points_positions = {1.0, 2.0, 3.0}; + std::vector jumble_map = {1, 2, 0}; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ - joint_names_[1], joint_names_[2], joint_names_[0]}; + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]], joint_names_[jumble_map[2]]}; + traj_msg.joint_names = jumbled_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); traj_msg.points[0].positions.resize(3); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 3.0; - traj_msg.points[0].positions[2] = 1.0; - - if (traj_controller_->has_velocity_command_interface()) + traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); + traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); + traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); + traj_msg.points[0].velocities.resize(3); + for (size_t i = 0; i < 3; i++) { - traj_msg.points[0].velocities.resize(3); - traj_msg.points[0].velocities[0] = -0.1; - traj_msg.points[0].velocities[1] = -0.1; - traj_msg.points[0].velocities[2] = -0.1; + // factor 2 comes from the linear interpolation in the spline trajectory for constant + // acceleration + traj_msg.points[0].velocities[i] = + 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; } + trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update for 0.25 seconds + // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, + // otherwise acceleration is zero from the spline trajectory) // TODO(destogl): Make this time a bit shorter to increase stability on the CI? // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25)); + updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { + // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling + // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } - // TODO(anyone): add here checks for acceleration commands + + if (traj_controller_->has_acceleration_command_interface()) + { + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } } /** @@ -1003,9 +1034,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); traj_msg.points[0].velocities[0] = - copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); traj_msg.points[0].velocities[1] = - copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); trajectory_publisher_->publish(traj_msg); } @@ -1027,22 +1058,50 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the velocity // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } + if (traj_controller_->has_acceleration_command_interface()) + { + // estimate the sign of the acceleration + // joint rotates forward + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + } + EXPECT_NEAR(0.0, joint_acc_[2], threshold) + << "Joint 3 acc should be 0.0 since it's not in the goal"; + } + if (traj_controller_->has_effort_command_interface()) { // estimate the sign of the effort // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); EXPECT_NEAR(0.0, joint_eff_[2], threshold) << "Joint 3 effort should be 0.0 since it's not in the goal"; } - // TODO(anyone): add here checks for acceleration commands executor.cancel(); } @@ -1284,7 +1343,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = - copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + std::copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5da7a35e1a..06e2f3e8db 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- 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 c947f00bdb..712d03068a 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 6e63a22917..9d5242684a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * add a broadcaster for range sensor (`#725 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 33ed7d5e4d..70d9c1fa2f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 3.15.0 + 3.16.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6684adfabc..d21abd09ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- * add a broadcaster for range sensor (`#725 `_) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ea2456486b..35cbdbf9bd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.15.0 + 3.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 3caadb3880..525d0a0221 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 845e65d233..398a279ae7 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.15.0 + 3.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e4d9a72f89..9b4e3a8b6b 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.15.0", + version="3.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 180dd35033..ecf0568408 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 9987061405..96b34a5609 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.15.0 + 3.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 0dea799d80..cf09679e00 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.15.0", + version="3.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 4c4dae365d..a7546cb59d 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 48c137460b..c7c654846a 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 3.15.0 + 3.16.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 86acb01dae..8acdfb1448 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -59,7 +59,7 @@ steering_controllers_library: open_loop: { type: bool, default_value: false, - description: "bool parameter decides if open oop or not (feedback).", + description: "Choose if open-loop or not (feedback) is used for odometry calculation.", read_only: false, } @@ -87,7 +87,7 @@ steering_controllers_library: enable_odom_tf: { type: bool, default_value: true, - description: "Publishing to tf is enabled or disabled ?.", + description: "Publishing to tf is enabled or disabled?", read_only: false, } @@ -108,15 +108,15 @@ steering_controllers_library: position_feedback: { type: bool, default_value: false, - description: "Choice of feedback type, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if - position_feedback is true then HW_IF_POSITION is taken as interface type", + description: "Choice of feedback type, if position_feedback is false then ``HW_IF_VELOCITY`` is taken as interface type, if + position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } use_stamped_vel: { type: bool, default_value: false, - description: "Choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if - use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if + use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", read_only: false, } diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 625c0f4a06..91175391f3 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index a53be880f2..ec4d5e064d 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.15.0 + 3.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index afe6360322..dd50c98185 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- + 3.15.0 (2023-09-11) ------------------- 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 f320c08c9f..64ebddd5e2 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 3.15.0 + 3.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 2551b76bd8..ce8bcfefa3 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.16.0 (2023-09-20) +------------------- +* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) +* Contributors: Christoph Fröhlich + 3.15.0 (2023-09-11) ------------------- 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 d0b6cf0ed1..e47e170226 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.15.0 + 3.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios