[JTC] Add time-out for trajectory interfaces (#609)

Author: christophfroehlich

joint_trajectory_controller/doc/parameters.rst

@@ -68,6 +68,16 @@ allow_nonzero_velocity_at_trajectory_end (boolean)
 
   Default: true
 
+cmd_timeout (double)
+  Timeout after which the input command is considered stale.
+  Timeout is counted from the end of the trajectory (the last point).
+  ``cmd_timeout`` must be greater than ``constraints.goal_time``,
+  otherwise ignored.
+
+  If zero, timeout is deactivated"
+
+  Default: 0.0
+
 constraints (structure)
   Default values for tolerances if no explicit values are states in JointTrajectory message.
 

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -174,7 +174,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
-  realtime_tools::RealtimeBuffer<bool> rt_is_holding_;  ///< are we holding position?
+  // Timeout to consider commands old
+  double cmd_timeout_;
+  // Are we holding position?
+  realtime_tools::RealtimeBuffer<bool> rt_is_holding_;
   // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -139,9 +139,6 @@ class Trajectory
     return trajectory_msg_;
   }
 
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; }
-
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool is_sampled_already() const { return sampled_already_; }
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -186,7 +186,7 @@ controller_interface::return_type JointTrajectoryController::update(
   state_current_.time_from_start.set__sec(0);
   read_state_from_hardware(state_current_);
 
-  // currently carrying out a trajectory.
+  // currently carrying out a trajectory
   if (has_active_trajectory())
   {
     bool first_sample = false;
@@ -211,12 +211,32 @@ controller_interface::return_type JointTrajectoryController::update(
 
     if (valid_point)
     {
+      const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
+      // this is the time instance
+      // - started with the first segment: when the first point will be reached (in the future)
+      // - later: when the point of the current segment was reached
+      const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start;
+      // time_difference is
+      // - negative until first point is reached
+      // - counting from zero to time_from_start of next point
+      double time_difference = time.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;
-      double time_difference = 0.0;
       const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end();
 
+      // have we reached the end, are not holding position, and is a timeout configured?
+      // Check independently of other tolerances
+      if (
+        !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 &&
+        time_difference > cmd_timeout_)
+      {
+        RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout");
+
+        traj_msg_external_point_ptr_.reset();
+        traj_msg_external_point_ptr_.initRT(set_hold_position());
+      }
+
       // Check state/goal tolerance
       for (size_t index = 0; index < dof_; ++index)
       {
@@ -243,12 +263,6 @@ controller_interface::return_type JointTrajectoryController::update(
 
           if (default_tolerances_.goal_time_tolerance != 0.0)
           {
-            // if we exceed goal_time_tolerance set it to aborted
-            const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time();
-            const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
-
-            time_difference = time.seconds() - traj_end.seconds();
-
             if (time_difference > default_tolerances_.goal_time_tolerance)
             {
               within_goal_time = false;
@@ -894,6 +908,26 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     std::string(get_node()->get_name()) + "/query_state",
     std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));
 
+  if (params_.cmd_timeout > 0.0)
+  {
+    if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
+    {
+      cmd_timeout_ = params_.cmd_timeout;
+    }
+    else
+    {
+      // deactivate timeout
+      RCLCPP_WARN(
+        logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)",
+        params_.cmd_timeout, default_tolerances_.goal_time_tolerance);
+      cmd_timeout_ = 0.0;
+    }
+  }
+  else
+  {
+    cmd_timeout_ = 0.0;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -83,6 +83,14 @@ joint_trajectory_controller:
     default_value: true,
     description: "If false, the last velocity point has to be zero or the goal will be rejected",
   }
+  cmd_timeout: {
+    type: double,
+    default_value: 0.0, # seconds
+    description: "Timeout after which the input command is considered stale.
+     Timeout is counted from the end of the trajectory (the last point).
+     cmd_timeout must be greater than constraints.goal_time, otherwise ignored.
+     If zero, timeout is deactivated",
+  }
   gains:
     __map_joints:
       p: {

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -565,6 +565,147 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
   executor.cancel();
 }
 
+/**
+ * @brief cmd_timeout must be greater than constraints.goal_time
+ */
+TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  // zero is default value, just for demonstration
+  double cmd_timeout = 3.0;
+  rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout);
+  rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0);
+  SetUpAndActivateTrajectoryController(
+    executor, {cmd_timeout_parameter, goal_time_parameter}, false);
+
+  EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout());
+}
+
+/**
+ * @brief cmd_timeout must be greater than constraints.goal_time
+ */
+TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  // zero is default value, just for demonstration
+  rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0);
+  rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0);
+  SetUpAndActivateTrajectoryController(
+    executor, {cmd_timeout_parameter, goal_time_parameter}, false);
+
+  EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout());
+}
+
+/**
+ * @brief check if no timeout is triggered
+ */
+TEST_P(TrajectoryControllerTestParameterized, no_timeout)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  // zero is default value, just for demonstration
+  rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0);
+  SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false);
+  subscribeToState();
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  // *INDENT-ON*
+  publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4);
+
+  // Spin to receive latest state
+  executor.spin_some();
+  auto state_msg = getState();
+  ASSERT_TRUE(state_msg);
+
+  // has the msg the correct vector sizes?
+  EXPECT_EQ(n_joints, state_msg->reference.positions.size());
+
+  // is the trajectory still active?
+  EXPECT_TRUE(traj_controller_->has_active_traj());
+  // should still hold the points from above
+  EXPECT_TRUE(traj_controller_->has_nontrivial_traj());
+  EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2);
+  EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2);
+  EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2);
+  // value of velocities is different from above due to spline interpolation
+  EXPECT_GT(state_msg->reference.velocities[0], 0.0);
+  EXPECT_GT(state_msg->reference.velocities[1], 0.0);
+  EXPECT_GT(state_msg->reference.velocities[2], 0.0);
+
+  executor.cancel();
+}
+
+/**
+ * @brief check if timeout is triggered
+ */
+TEST_P(TrajectoryControllerTestParameterized, timeout)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  constexpr double cmd_timeout = 0.1;
+  rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout);
+  double kp = 1.0;  // activate feedback control for testing velocity/effort PID
+  SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp);
+  subscribeToState();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  // *INDENT-ON*
+
+  publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // update until end of trajectory -> no timeout should have occurred
+  updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3);
+  // is a trajectory active?
+  EXPECT_TRUE(traj_controller_->has_active_traj());
+  // should have the trajectory with three points
+  EXPECT_TRUE(traj_controller_->has_nontrivial_traj());
+
+  // update until timeout should have happened
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // Spin to receive latest state
+  executor.spin_some();
+  auto state_msg = getState();
+  ASSERT_TRUE(state_msg);
+
+  // after timeout, set_hold_position adds a new trajectory
+  // is a trajectory active?
+  EXPECT_TRUE(traj_controller_->has_active_traj());
+  // should be not more than one point now (from hold position)
+  EXPECT_FALSE(traj_controller_->has_nontrivial_traj());
+  // should hold last position with zero velocity
+  if (traj_controller_->has_position_command_interface())
+  {
+    expectHoldingPoint(points.at(2));
+  }
+  else
+  {
+    // no integration to position state interface from velocity/acceleration
+    expectHoldingPoint(INITIAL_POS_JOINTS);
+  }
+
+  executor.cancel();
+}
+
 /**
  * @brief check if position error of revolute joints are normalized if configured so
  */

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -137,6 +137,13 @@ class TestableJointTrajectoryController
     return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false;
   }
 
+  bool has_nontrivial_traj()
+  {
+    return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg();
+  }
+
+  double get_cmd_timeout() { return cmd_timeout_; }
+
   rclcpp::WaitSet joint_cmd_sub_wait_set_;
 };