Merge branch 'master' into tl-expected

Author: christophfroehlich

doc/migration.rst

@@ -8,6 +8,12 @@ joint_state_broadcaster
 *****************************
 * Removed interfaces with other data types than double for publishing to ``dynamic_joint_states``. (`#2115 <https://github.com/ros-controls/ros2_controllers/pull/2115>`_).
   Use a custom controller for publishing non-double interfaces.
+* Parameter ``publish_dynamic_joint_states`` is now deprecated (default changed to ``false``). (`#2107 <https://github.com/ros-controls/ros2_controllers/pull/2107>`_)
+  For publishing non-standard interfaces, consider using alternatives:
+
+  * :ref:`state_interfaces_broadcaster <state_interfaces_broadcaster_userdoc>` for broadcasting arbitrary state interfaces
+  * :ref:`gpio_controllers <gpio_controllers_userdoc>` for GPIO and custom hardware interfaces
+  * `pal_statistics <https://github.com/pal-robotics/pal_statistics>`_ for flexible runtime statistics publishing
 
 effort_controllers
 *****************************

doc/release_notes.rst

@@ -30,6 +30,7 @@ joint_state_broadcaster
 * Make all parameters read-only (the never got re-evaluated after initialization anyways). (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
 * Added parameter ``publish_dynamic_joint_states`` to enable/disable publishing of dynamic joint states. (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
 * Removed interfaces with other data types than double for publishing to ``dynamic_joint_states``. (`#2115 <https://github.com/ros-controls/ros2_controllers/pull/2115>`_)
+* Parameter ``publish_dynamic_joint_states`` is now deprecated (default changed to ``false``).
 
 omni_wheel_drive_controller
 *****************************
@@ -43,6 +44,7 @@ joint_trajectory_controller
   don't contain velocity / acceleration information, but the trajectory does. This way, the segment
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
   <https://github.com/ros-controls/ros2_controllers/pull/2043>`_)
+* Added decelerate-to-stop functionality when a trajectory is canceled or preempted. Instead of immediately holding position, the controller can now smoothly decelerate each joint to a stop using the per-joint ``max_deceleration_on_cancel`` parameter. (`#2163 <https://github.com/ros-controls/ros2_controllers/pull/2163>`_)
 
 pid_controller
 **************

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -92,6 +92,14 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
 {
   params_ = param_listener_->get_params();
 
+  if (params_.publish_dynamic_joint_states)
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "[Deprecated] The 'publish_dynamic_joint_states' parameter is deprecated and will be removed "
+      "in future releases. Please update your configuration.");
+  }
+
   if (use_all_available_interfaces())
   {
     RCLCPP_INFO(
@@ -420,8 +428,13 @@ controller_interface::return_type JointStateBroadcaster::update(
       const auto & opt = state_interfaces_[i].get_optional(0);
       if (opt.has_value())
       {
-        *mapped_values_[map_index++] = opt.value();
+        *mapped_values_[map_index] = opt.value();
       }
+      // Always advance map_index for every DOUBLE interface, regardless of whether the read
+      // succeeded. If we only advance on success, a temporary read failure (e.g. lock contention
+      // on a chained interface) causes all subsequent interfaces to be written into the wrong
+      // mapped_values_ slots, corrupting the published joint states.
+      ++map_index;
     }
   }
 

joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml

@@ -62,7 +62,7 @@ joint_state_broadcaster:
   }
   publish_dynamic_joint_states: {
     type: bool,
-    default_value: true,
+    default_value: false,
     read_only: true,
-    description: "Whether to publish dynamic joint states."
+    description: "[Deprecated] Whether to publish dynamic joint states. This parameter is deprecated and will be removed in future releases."
   }

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -34,7 +34,6 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
 {
   FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest);
-  FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyWithoutDynamicJointStatesPublisherTest);
   FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF);
@@ -52,6 +51,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
   FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest);
   FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest);
   FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanAndDoubleInterfaceTest);
+  FRIEND_TEST(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest);
 };
 
 class JointStateBroadcasterTest : public ::testing::Test
@@ -79,8 +79,6 @@ class JointStateBroadcasterTest : public ::testing::Test
 
   void test_published_joint_state_message(const std::string & topic);
 
-  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);
 

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -103,7 +103,7 @@ if(BUILD_TESTING)
   ament_add_gmock(test_trajectory_actions
     test/test_trajectory_actions.cpp
   )
-  set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
+  set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 330)
   target_link_libraries(test_trajectory_actions
     joint_trajectory_controller
     ros2_control_test_assets::ros2_control_test_assets)

joint_trajectory_controller/CMakeLists.txt

@@ -0,0 +1,83 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/decelerate_on_cancel.rst
+
+.. _joint_trajectory_controller_decelerate_on_cancel:
+
+Decelerate on cancel
+====================
+
+By default, when a trajectory is canceled or preempted, the controller immediately holds the current position.
+This can cause problems for hardware that is moving at high velocity, as the abrupt stop may trigger faults or cause excessive wear.
+
+When the ``decelerate_on_cancel`` feature is enabled, the controller instead generates a smooth stop trajectory that decelerates each joint to zero velocity using a constant deceleration profile before holding position.
+
+.. image:: decelerate_hold_position.png
+   :alt: Default behavior: immediate hold position on cancel
+
+*Default behavior: the controller instantly sets the command position to the current position when a trajectory is canceled, causing an abrupt stop.*
+
+.. image:: decelerate_to_stop.png
+   :alt: Decelerate to stop behavior on cancel
+
+*With decelerate on cancel enabled: the controller generates a ramped stop trajectory that smoothly decelerates each joint before holding position.*
+
+How it works
+------------
+
+When a trajectory is canceled or preempted and ``decelerate_on_cancel`` is enabled, the controller:
+
+1. Reads the current velocity of each joint from the velocity state interface.
+2. Computes the stopping distance and time for each joint using its configured ``max_deceleration_on_cancel`` value:
+
+   .. math::
+
+      t_{stop} = \frac{|v|}{a_{max}}
+
+   .. math::
+
+      d_{stop} = \frac{v \cdot t_{stop}}{2}
+
+   where :math:`v` is the current velocity and :math:`a_{max}` is the configured maximum deceleration.
+
+3. Generates a trajectory with intermediate waypoints that linearly ramp velocity to zero over the computed stopping time.
+4. Appends a final hold-position point at the computed stop position.
+
+Each joint decelerates independently based on its own ``max_deceleration_on_cancel`` value, but the trajectory is synchronized so all joints finish at the same time (the slowest joint's stop time).
+
+Requirements
+------------
+
+* The hardware must provide a ``velocity`` state interface for all joints in the controller. If velocity state feedback is not available, the controller falls back to the default hold-position behavior.
+* Each joint must have a valid (greater than zero) ``max_deceleration_on_cancel`` value. Joints with a value of ``0.0`` cause the controller to fall back to hold-position behavior.
+
+Configuration
+-------------
+
+Enable the feature by setting ``constraints.decelerate_on_cancel`` to ``true`` and providing a ``max_deceleration_on_cancel`` value (in rad/s^2 or m/s^2) for each joint under ``constraints.<joint_name>``:
+
+.. code-block:: yaml
+
+   controller_name:
+     ros__parameters:
+       joints:
+         - joint_1
+         - joint_2
+         - joint_3
+
+       constraints:
+         decelerate_on_cancel: true
+         stopped_velocity_tolerance: 0.01
+         goal_time: 0.0
+         joint_1:
+           max_deceleration_on_cancel: 10.0
+         joint_2:
+           max_deceleration_on_cancel: 3.0
+         joint_3:
+           max_deceleration_on_cancel: 6.0
+
+.. note::
+
+   Both ``decelerate_on_cancel`` and ``max_deceleration_on_cancel`` are read-only parameters. They can only be set at controller configuration time and cannot be changed at runtime.
+
+.. note::
+
+   Choose ``max_deceleration_on_cancel`` values that are within the physical limits of your hardware. Values that are too high may still cause faults, while values that are too low will result in longer stopping distances.