From 0a9f4b0c2b10c926d6c283520d763eefed0f06b5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 16 Nov 2023 23:17:20 +0000 Subject: [PATCH 1/4] Bump ros-tooling/action-ros-ci from 0.3.4 to 0.3.5 (#833) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 46922ccaac..76b2c22e53 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 723654b33e..1aaf30c639 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6a932a2dcd..72aa9af66c 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index a444a7f645..6b1af2b86c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From 25f2a14757e5624eabfd5804fb64b42100e72b52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 15:31:39 +0100 Subject: [PATCH 2/4] Resort overview page (#846) --- README.md | 2 +- doc/controllers_index.rst | 55 ++++++++++++---------- forward_command_controller/doc/userdoc.rst | 2 +- 3 files changed, 32 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index d41ec09b6c..4183b38fdc 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) -Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Navigation2. +Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. ## Build status diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index de33c3284c..9fceb90fd3 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -6,24 +6,9 @@ ros2_controllers ################# -`GitHub Repository `_ - - -Nomenclature -************ - -The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using. -The controllers are using `common hardware interface definitions`_. -The controllers' namespaces are commanding the following command interface types: - - - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` - - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` - - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` - - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - - ... - -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. +`Link to GitHub Repository `_ Guidelines and Best Practices @@ -36,34 +21,54 @@ Guidelines and Best Practices * -Available Controllers -********************* +Controllers for Mobile Robots +***************************** .. toctree:: :titlesonly: Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Admittance Controller <../admittance_controller/doc/userdoc.rst> Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> + +Controllers for Manipulators and Other Robots +********************************************* + +The controllers are using `common hardware interface definitions`_, and may use namespaces depending on the following command interface types: + + - ``position_controllers``: ``hardware_interface::HW_IF_POSITION`` + - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY`` + - ``effort_controllers``: ``hardware_interface::HW_IF_ACCELERATION`` + - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` + +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp + + +.. toctree:: + :titlesonly: + + Admittance Controller <../admittance_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> - Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> - Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> -Available Broadcasters +Broadcasters ********************** +Broadcasters are used to publish sensor data from hardware components to ROS topics. +In the sense of ros2_control, broadcasters are still controllers using the same controller interface as the other controllers above. + .. toctree:: :titlesonly: Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> - Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 01aa2492a8..cd623a5acb 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ forward_command_controller ========================== -This is a base class implementing a feedforward controller. Specific implementations can be found in: +This is a base class implementing a feedforward controller. Specific implementations of this base class can be found in: * :ref:`position_controllers_userdoc` * :ref:`velocity_controllers_userdoc` From 41610fdf43b9875f079aa5933b3f95e9687e524d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:17:24 +0100 Subject: [PATCH 3/4] [JTC] Remove unused home pose (#845) --- .../joint_trajectory_controller.hpp | 2 -- .../src/joint_trajectory_controller.cpp | 28 ------------------- 2 files changed, 30 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 7542aa6951..397ccf347c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -184,8 +184,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3936da155f..18f61f0e49 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -921,22 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); @@ -1028,8 +1013,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return CallbackReturn::SUCCESS; } @@ -1037,13 +1020,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { - // go home - if (traj_home_point_ptr_ != nullptr) - { - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_external_point_ptr_ = traj_home_point_ptr_; - } - return CallbackReturn::SUCCESS; } @@ -1070,11 +1046,7 @@ bool JointTrajectoryController::reset() } } - // iterator has no default value - // prev_traj_point_ptr_; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return true; } From d9a6bb43e312d8d74e52eddfb700e5249c18f4f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:18:46 +0100 Subject: [PATCH 4/4] [JTC] Fix dynamic reconfigure of tolerances (#849) * Fix dynamic reconfigure of tolerances * Fix static cast syntax --- .../src/joint_trajectory_controller.cpp | 5 +- .../test/test_trajectory_controller.cpp | 53 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 5 ++ 3 files changed, 61 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 18f61f0e49..dae7f13148 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -119,11 +119,12 @@ controller_interface::return_type JointTrajectoryController::update( if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - // use_closed_loop_pid_adapter_ is updated in on_configure only + default_tolerances_ = get_segment_tolerances(params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) { update_pids(); - default_tolerances_ = get_segment_tolerances(params_); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7842d50434..8b29c2cb16 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -449,6 +449,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) executor.cancel(); } +/** + * @brief check if dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.joint1.trajectory", 1.0), + rclcpp::Parameter("constraints.joint2.trajectory", 2.0), + rclcpp::Parameter("constraints.joint3.trajectory", 3.0), + rclcpp::Parameter("constraints.joint1.goal", 10.0), + rclcpp::Parameter("constraints.joint2.goal", 20.0), + rclcpp::Parameter("constraints.joint3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateController(); + + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 224265ad83..fdf4305cec 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -132,6 +132,11 @@ class TestableJointTrajectoryController std::vector get_pids() const { return pids_; } + joint_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const