Merge branch 'master' into jtc/action_tolerances

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.
 
@@ -102,7 +112,7 @@ gains (structure)
 
      u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v)
 
-  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
+  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below),
   the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
 
 gains.<joint_name>.p (double)
@@ -130,10 +140,12 @@ gains.<joint_name>.ff_velocity_scale (double)
 
   Default: 0.0
 
-gains.<joint_name>.normalize_error (bool)
+gains.<joint_name>.angle_wraparound (bool)
+  For joints that wrap around (without end stop, ie. are continuous),
+  where the shortest rotation to the target position is the desired motion.
   If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`.
   Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
-  position :math:`s` from the state interface. Use this for revolute joints without end stop,
-  where the shortest rotation to the target position is the desired motion.
+  position :math:`s` from the state interface.
+
 
   Default: false

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,13 +211,33 @@ 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();
       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
+      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)
       {
@@ -727,12 +747,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     }
   }
 
-  // Configure joint position error normalization from ROS parameters
+  // Configure joint position error normalization from ROS parameters (angle_wraparound)
   normalize_joint_error_.resize(dof_);
   for (size_t i = 0; i < dof_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
-    normalize_joint_error_[i] = gains.normalize_error;
+    if (gains.normalize_error)
+    {
+      // TODO(anyone): Remove deprecation warning in the end of 2023
+      RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!");
+      normalize_joint_error_[i] = gains.normalize_error;
+    }
+    else
+    {
+      normalize_joint_error_[i] = gains.angle_wraparound;
+    }
   }
 
   if (params_.state_interfaces.empty())
@@ -886,6 +915,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: {
@@ -113,7 +121,13 @@ joint_trajectory_controller:
       normalize_error: {
         type: bool,
         default_value: false,
-        description: "Use position error normalization to -pi to pi."
+        description: "(Deprecated) Use position error normalization to -pi to pi."
+      }
+      angle_wraparound: {
+        type: bool,
+        default_value: false,
+        description: "For joints that wrap around (ie. are continuous).
+          Normalizes position-error to -pi to pi."
       }
   constraints:
     stopped_velocity_tolerance: {

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

@@ -142,6 +142,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_;
 };