[JTC] Process tolerances sent with action goal (ros-controls#716)

doc/migration/Jazzy.rst

@@ -18,3 +18,4 @@ joint_trajectory_controller
 * Empty trajectory messages are discarded (`#902 <https://github.com/ros-controls/ros2_controllers/pull/902>`_).
 * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
 * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint.
+* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). Adaptions to the action goal might be necessary.

doc/release_notes/Jazzy.rst

@@ -33,6 +33,20 @@ joint_trajectory_controller
 * Action field ``error_string`` is now filled with meaningful strings (`#887 <https://github.com/ros-controls/ros2_controllers/pull/887>`_).
 * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
 * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). ``angle_wraparound`` parameter was completely removed.
+* Tolerances sent with the action goal are now processed and used for the action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
+
+  .. code-block:: markdown
+
+    The tolerances specify the amount the position, velocity, and
+    accelerations can vary from the setpoints.  For example, in the case
+    of trajectory control, when the actual position varies beyond
+    (desired position + position tolerance), the trajectory goal may
+    abort.
+
+    There are two special values for tolerances:
+     * 0 - The tolerance is unspecified and will remain at whatever the default is
+     * -1 - The tolerance is "erased".  If there was a default, the joint will be
+            allowed to move without restriction.
 
 pid_controller
 ************************

joint_trajectory_controller/CMakeLists.txt

@@ -61,6 +61,10 @@ if(BUILD_TESTING)
   target_link_libraries(test_trajectory joint_trajectory_controller)
   target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
 
+  ament_add_gmock(test_tolerances test/test_tolerances.cpp)
+  target_link_libraries(test_tolerances joint_trajectory_controller)
+  target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES)
+
   ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp)
   set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)

joint_trajectory_controller/doc/userdoc.rst

@@ -152,7 +152,21 @@ Actions  [#f1]_
 
 The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired.
 
-Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances.
+Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
+
+.. code-block:: markdown
+
+  The tolerances specify the amount the position, velocity, and
+  accelerations can vary from the setpoints.  For example, in the case
+  of trajectory control, when the actual position varies beyond
+  (desired position + position tolerance), the trajectory goal may
+  abort.
+
+  There are two special values for tolerances:
+    * 0 - The tolerance is unspecified and will remain at whatever the default is
+    * -1 - The tolerance is "erased".  If there was a default, the joint will be
+          allowed to move without restriction.
+
 When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`).
 If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held.
 

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -244,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     size_t joint_names_size, const std::vector<double> & vector_field,
     const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
 
+  // the tolerances from the node parameter
   SegmentTolerances default_tolerances_;
+  // the tolerances used for the current goal
+  realtime_tools::RealtimeBuffer<SegmentTolerances> active_tolerances_;
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void preempt_active_goal();

joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

@@ -30,12 +30,15 @@
 #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 
+#include <limits>
+#include <string>
 #include <vector>
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "joint_trajectory_controller_parameters.hpp"
 
 #include "rclcpp/node.hpp"
+#include "rclcpp/time.hpp"
 
 namespace joint_trajectory_controller
 {
@@ -85,16 +88,19 @@ struct SegmentTolerances
  *    goal: 0.01
  * \endcode
  *
+ * \param jtc_logger The logger to use for output
  * \param params The ROS Parameters
  * \return Trajectory segment tolerances.
  */
-SegmentTolerances get_segment_tolerances(Params const & params)
+SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params)
 {
   auto const & constraints = params.constraints;
   auto const n_joints = params.joints.size();
 
   SegmentTolerances tolerances;
   tolerances.goal_time_tolerance = constraints.goal_time;
+  static auto logger = jtc_logger.get_child("tolerance");
+  RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time);
 
   // State and goal state tolerances
   tolerances.state_tolerance.resize(n_joints);
@@ -106,16 +112,221 @@ SegmentTolerances get_segment_tolerances(Params const & params)
     tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal;
     tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance;
 
-    auto logger = rclcpp::get_logger("tolerance");
     RCLCPP_DEBUG(
-      logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position);
+      logger, "%s %f", (joint + ".trajectory.position").c_str(),
+      tolerances.state_tolerance[i].position);
     RCLCPP_DEBUG(
-      logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position);
+      logger, "%s %f", (joint + ".goal.position").c_str(),
+      tolerances.goal_state_tolerance[i].position);
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".goal.velocity").c_str(),
+      tolerances.goal_state_tolerance[i].velocity);
   }
 
   return tolerances;
 }
 
+/**
+ * \brief Populate trajectory segment tolerances using data from an action goal.
+ *
+ * \param jtc_logger The logger to use for output
+ * \param default_tolerances The default tolerances to use if the action goal does not specify any.
+ * \param goal The new action goal
+ * \param joints The joints configured by ROS parameters
+ * \return Trajectory segment tolerances.
+ */
+SegmentTolerances get_segment_tolerances(
+  rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances,
+  const control_msgs::action::FollowJointTrajectory::Goal & goal,
+  const std::vector<std::string> & joints)
+{
+  SegmentTolerances active_tolerances(default_tolerances);
+
+  active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds();
+  static auto logger = jtc_logger.get_child("tolerance");
+  RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance);
+
+  // from
+  // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg
+  // There are two special values for tolerances:
+  // * 0 - The tolerance is unspecified and will remain at whatever the default is
+  // * -1 - The tolerance is "erased".
+  //        If there was a default, the joint will be allowed to move without restriction.
+  constexpr double ERASE_VALUE = -1.0;
+  auto is_erase_value = [](double value)
+  { return fabs(value - ERASE_VALUE) < std::numeric_limits<float>::epsilon(); };
+
+  // State and goal state tolerances
+  for (auto joint_tol : goal.path_tolerance)
+  {
+    auto const joint = joint_tol.name;
+    // map joint names from goal to active_tolerances
+    auto it = std::find(joints.begin(), joints.end(), joint);
+    if (it == joints.end())
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.path_tolerance does not exist. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+    auto i = std::distance(joints.cbegin(), it);
+    if (joint_tol.position > 0.0)
+    {
+      active_tolerances.state_tolerance[i].position = joint_tol.position;
+    }
+    else if (is_erase_value(joint_tol.position))
+    {
+      active_tolerances.state_tolerance[i].position = 0.0;
+    }
+    else if (joint_tol.position < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.path_tolerance has a invalid position tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    if (joint_tol.velocity > 0.0)
+    {
+      active_tolerances.state_tolerance[i].velocity = joint_tol.velocity;
+    }
+    else if (is_erase_value(joint_tol.velocity))
+    {
+      active_tolerances.state_tolerance[i].velocity = 0.0;
+    }
+    else if (joint_tol.velocity < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.path_tolerance has a invalid velocity tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    if (joint_tol.acceleration > 0.0)
+    {
+      active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration;
+    }
+    else if (is_erase_value(joint_tol.acceleration))
+    {
+      active_tolerances.state_tolerance[i].acceleration = 0.0;
+    }
+    else if (joint_tol.acceleration < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.path_tolerance has a invalid acceleration tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".state_tolerance.position").c_str(),
+      active_tolerances.state_tolerance[i].position);
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(),
+      active_tolerances.state_tolerance[i].velocity);
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(),
+      active_tolerances.state_tolerance[i].acceleration);
+  }
+  for (auto goal_tol : goal.goal_tolerance)
+  {
+    auto const joint = goal_tol.name;
+    // map joint names from goal to active_tolerances
+    auto it = std::find(joints.begin(), joints.end(), joint);
+    if (it == joints.end())
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.goal_tolerance does not exist. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+    auto i = std::distance(joints.cbegin(), it);
+    if (goal_tol.position > 0.0)
+    {
+      active_tolerances.goal_state_tolerance[i].position = goal_tol.position;
+    }
+    else if (is_erase_value(goal_tol.position))
+    {
+      active_tolerances.goal_state_tolerance[i].position = 0.0;
+    }
+    else if (goal_tol.position < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.goal_tolerance has a invalid position tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    if (goal_tol.velocity > 0.0)
+    {
+      active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity;
+    }
+    else if (is_erase_value(goal_tol.velocity))
+    {
+      active_tolerances.goal_state_tolerance[i].velocity = 0.0;
+    }
+    else if (goal_tol.velocity < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.goal_tolerance has a invalid velocity tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    if (goal_tol.acceleration > 0.0)
+    {
+      active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration;
+    }
+    else if (is_erase_value(goal_tol.acceleration))
+    {
+      active_tolerances.goal_state_tolerance[i].acceleration = 0.0;
+    }
+    else if (goal_tol.acceleration < 0.0)
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint '" + joint +
+         "' specified in goal.goal_tolerance has a invalid acceleration tolerance. "
+         "Using default tolerances.")
+          .c_str());
+      return default_tolerances;
+    }
+
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(),
+      active_tolerances.goal_state_tolerance[i].position);
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(),
+      active_tolerances.goal_state_tolerance[i].velocity);
+    RCLCPP_DEBUG(
+      logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(),
+      active_tolerances.goal_state_tolerance[i].acceleration);
+  }
+
+  return active_tolerances;
+}
+
 /**
  * \param state_error State error to check.
  * \param joint_idx Joint index for the state error