[JTC] Process tolerances sent with action goal

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,14 @@
 #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 
+#include <limits>
 #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
 {
@@ -95,6 +97,8 @@ SegmentTolerances get_segment_tolerances(Params const & params)
 
   SegmentTolerances tolerances;
   tolerances.goal_time_tolerance = constraints.goal_time;
+  auto logger = rclcpp::get_logger("tolerance");
+  RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time);
 
   // State and goal state tolerances
   tolerances.state_tolerance.resize(n_joints);
@@ -106,16 +110,153 @@ 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 default_tolerances The default tolerances to use if the action goal does not specify any.
+ * \param goal The new action goal
+ * \param params The ROS Parameters
+ * \return Trajectory segment tolerances.
+ */
+SegmentTolerances get_segment_tolerances(
+  SegmentTolerances const & default_tolerances,
+  control_msgs::action::FollowJointTrajectory::Goal const & goal, Params const & params)
+{
+  SegmentTolerances active_tolerances(default_tolerances);
+
+  active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds();
+  auto logger = rclcpp::get_logger("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(params.joints.begin(), params.joints.end(), joint);
+    if (it == params.joints.end())
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint " + joint + "specified in goal.path_tolerance does not exist").c_str());
+      return active_tolerances;
+    }
+    auto i = std::distance(params.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;
+    }
+
+    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;
+    }
+
+    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;
+    }
+
+    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(params.joints.begin(), params.joints.end(), joint);
+    if (it == params.joints.end())
+    {
+      RCLCPP_ERROR(
+        logger, "%s",
+        ("joint " + joint + "specified in goal.goal_tolerance does not exist").c_str());
+      return active_tolerances;
+    }
+    auto i = std::distance(params.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;
+    }
+
+    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;
+    }
+
+    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;
+    }
+
+    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

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -200,6 +200,7 @@ controller_interface::return_type JointTrajectoryController::update(
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;
       const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end();
+      auto active_tol = active_tolerances_.readFromRT();
 
       // have we reached the end, are not holding position, and is a timeout configured?
       // Check independently of other tolerances
@@ -224,23 +225,25 @@ controller_interface::return_type JointTrajectoryController::update(
         if (
           (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false &&
           !check_state_tolerance_per_joint(
-            state_error_, index, default_tolerances_.state_tolerance[index],
-            true /* show_errors */))
+            state_error_, index, active_tol->state_tolerance[index], true /* show_errors */) &&
+          *(rt_is_holding_.readFromRT()) == false)
         {
           tolerance_violated_while_moving = true;
         }
         // past the final point, check that we end up inside goal tolerance
         if (
           !before_last_point && *(rt_is_holding_.readFromRT()) == false &&
           !check_state_tolerance_per_joint(
-            state_error_, index, default_tolerances_.goal_state_tolerance[index],
-            false /* show_errors */))
+            state_error_, index, active_tol->goal_state_tolerance[index],
+            false /* show_errors */) &&
+          *(rt_is_holding_.readFromRT()) == false)
         {
           outside_goal_tolerance = true;
 
-          if (default_tolerances_.goal_time_tolerance != 0.0)
+          if (active_tol->goal_time_tolerance != 0.0)
           {
-            if (time_difference > default_tolerances_.goal_time_tolerance)
+            // if we exceed goal_time_tolerance set it to aborted
+            if (time_difference > active_tol->goal_time_tolerance)
             {
               within_goal_time = false;
               // print once, goal will be aborted afterwards
@@ -782,6 +785,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     get_interface_list(params_.state_interfaces).c_str());
 
   // parse remaining parameters
+  default_tolerances_ = get_segment_tolerances(params_);
+  active_tolerances_.initRT(default_tolerances_);
   const std::string interpolation_string =
     get_node()->get_parameter("interpolation_method").as_string();
   interpolation_method_ = interpolation_methods::from_string(interpolation_string);
@@ -1173,6 +1178,10 @@ void JointTrajectoryController::goal_accepted_callback(
   rt_goal->execute();
   rt_active_goal_.writeFromNonRT(rt_goal);
 
+  // Update tolerances if specified in the goal
+  active_tolerances_.writeFromNonRT(
+    get_segment_tolerances(default_tolerances_, *(goal_handle->get_goal()), params_));
+
   // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list
   goal_handle_timer_.reset();