Merge branch 'master' into jtc/remove_start_with_holding

.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 }}

.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 }}

.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 }}

.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 }}

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
 

doc/controllers_index.rst

@@ -6,24 +6,9 @@
 ros2_controllers
 #################
 
-`GitHub Repository <https://github.com/ros-controls/ros2_controllers>`_
-
-
-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 <https://moveit.picknik.ai/main/index.html>`_ and `Nav2 <https://navigation.ros.org/>`_.
 
+`Link to GitHub Repository <https://github.com/ros-controls/ros2_controllers>`_
 
 
 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>

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`

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -184,8 +184,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   rclcpp::Service<control_msgs::srv::QueryTrajectoryState>::SharedPtr query_state_srv_;
 
   std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
-  std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
-  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
     traj_msg_external_point_ptr_;
 

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_);
     }
   }
 
@@ -921,22 +922,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     }
   }
 
-  // Store 'home' pose
-  traj_msg_home_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
-  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<Trajectory>();
-  traj_home_point_ptr_ = std::make_shared<Trajectory>();
   traj_msg_external_point_ptr_.writeFromNonRT(
     std::shared_ptr<trajectory_msgs::msg::JointTrajectory>());
 
@@ -1025,22 +1011,13 @@ 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;
 }
 
 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;
 }
 
@@ -1067,11 +1044,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;
 }

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -449,6 +449,77 @@ 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<rclcpp::Parameter> 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<double>(i) + 1.0);
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast<double>(i) + 1.0));
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02);
+    }
+  }
+
+  executor.cancel();
+}
+
+/**
+ * @brief check if hold on startup is deactivated
+ */
+TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+
+  rclcpp::Parameter start_with_holding_parameter("start_with_holding", false);
+  SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter});
+
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+  // after startup without start_with_holding being set, we expect no active trajectory
+  ASSERT_FALSE(traj_controller_->has_active_traj());
+
+  executor.cancel();
+}
+
 /**
  * @brief check if hold on startup
  */

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -132,6 +132,11 @@ class TestableJointTrajectoryController
 
   std::vector<PidPtr> 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