[JTC] Make goal_time_tolerance overwrite default value only if explicitly set (#1192)

Author: fmauch

joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

@@ -31,6 +31,7 @@
 #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 
 #include <limits>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -126,6 +127,33 @@ SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Para
   return tolerances;
 }
 
+double resolve_tolerance_source(const double default_value, const double goal_value)
+{
+  // 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(); };
+
+  if (goal_value > 0.0)
+  {
+    return goal_value;
+  }
+  else if (is_erase_value(goal_value))
+  {
+    return 0.0;
+  }
+  else if (goal_value < 0.0)
+  {
+    throw std::runtime_error("Illegal tolerance value.");
+  }
+  return default_value;
+}
+
 /**
  * \brief Populate trajectory segment tolerances using data from an action goal.
  *
@@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances(
   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(); };
+  try
+  {
+    active_tolerances.goal_time_tolerance = resolve_tolerance_source(
+      default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds());
+  }
+  catch (const std::runtime_error & e)
+  {
+    RCLCPP_ERROR_STREAM(
+      logger, "Specified illegal goal_time_tolerance: "
+                << rclcpp::Duration(goal.goal_time_tolerance).seconds()
+                << ". Using default tolerances");
+    return default_tolerances;
+  }
+  RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance);
 
   // State and goal state tolerances
   for (auto joint_tol : goal.path_tolerance)
@@ -173,60 +203,24 @@ SegmentTolerances get_segment_tolerances(
       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))
+    std::string interface = "";
+    try
     {
-      active_tolerances.state_tolerance[i].position = 0.0;
+      interface = "position";
+      active_tolerances.state_tolerance[i].position = resolve_tolerance_source(
+        default_tolerances.state_tolerance[i].position, joint_tol.position);
+      interface = "velocity";
+      active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source(
+        default_tolerances.state_tolerance[i].velocity, joint_tol.velocity);
+      interface = "acceleration";
+      active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source(
+        default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration);
     }
-    else if (joint_tol.position < 0.0)
+    catch (const std::runtime_error & e)
     {
-      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());
+      RCLCPP_ERROR_STREAM(
+        logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid "
+                          << interface << " tolerance. Using default tolerances.");
       return default_tolerances;
     }
 
@@ -256,60 +250,24 @@ SegmentTolerances get_segment_tolerances(
       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))
+    std::string interface = "";
+    try
     {
-      active_tolerances.goal_state_tolerance[i].velocity = 0.0;
+      interface = "position";
+      active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source(
+        default_tolerances.goal_state_tolerance[i].position, goal_tol.position);
+      interface = "velocity";
+      active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source(
+        default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity);
+      interface = "acceleration";
+      active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source(
+        default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration);
     }
-    else if (goal_tol.velocity < 0.0)
+    catch (const std::runtime_error & e)
     {
-      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());
+      RCLCPP_ERROR_STREAM(
+        logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid "
+                          << interface << " tolerance. Using default tolerances.");
       return default_tolerances;
     }
 

joint_trajectory_controller/test/test_tolerances.cpp

@@ -183,7 +183,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances)
   path_tolerance.push_back(tolerance);
   goal_tolerance.push_back(tolerance);
 
-  auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance);
+  auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance);
   auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
     logger, default_tolerances, goal_msg, params.joints);
 
@@ -215,6 +215,31 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances)
 // send goal with invalid tolerances, are the default ones used?
 TEST_F(TestTolerancesFixture, test_invalid_tolerances)
 {
+  {
+    SCOPED_TRACE("negative goal_time_tolerance");
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point;
+    point.time_from_start = rclcpp::Duration::from_seconds(0.5);
+    point.positions.resize(joint_names_.size());
+
+    point.positions[0] = 1.0;
+    point.positions[1] = 2.0;
+    point.positions[2] = 3.0;
+    points.push_back(point);
+
+    std::vector<control_msgs::msg::JointTolerance> path_tolerance;
+    control_msgs::msg::JointTolerance tolerance;
+    tolerance.name = "joint1";
+    tolerance.position = 0.0;
+    tolerance.velocity = 0.0;
+    tolerance.acceleration = 0.0;
+    path_tolerance.push_back(tolerance);
+
+    auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance);
+    auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
+      logger, default_tolerances, goal_msg, params.joints);
+    expectDefaultTolerances(active_tolerances);
+  }
   {
     SCOPED_TRACE("negative path position tolerance");
     std::vector<JointTrajectoryPoint> points;
@@ -238,7 +263,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
     auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
   {
@@ -264,7 +288,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
     auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
   {
@@ -290,7 +313,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
     auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
   {
@@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
       prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), goal_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
   {
@@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
       prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), goal_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
   {
@@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
       prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), goal_tolerance);
     auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
       logger, default_tolerances, goal_msg, params.joints);
-    EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
     expectDefaultTolerances(active_tolerances);
   }
 }
@@ -395,7 +414,6 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance)
   auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance);
   auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
     logger, default_tolerances, goal_msg, params.joints);
-  EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
   expectDefaultTolerances(active_tolerances);
 }
 TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance)
@@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance)
     prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), goal_tolerance);
   auto active_tolerances = joint_trajectory_controller::get_segment_tolerances(
     logger, default_tolerances, goal_msg, params.joints);
-  EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
   expectDefaultTolerances(active_tolerances);
 }

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -45,6 +45,7 @@ const double stopped_velocity_tolerance = 0.1;
 [[maybe_unused]] void expectDefaultTolerances(
   joint_trajectory_controller::SegmentTolerances active_tolerances)
 {
+  EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time);
   // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance
 
   ASSERT_EQ(active_tolerances.state_tolerance.size(), 3);