Implement timeout

Author: christophfroehlich

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -174,6 +174,11 @@ 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_;
 
+  // Timeout to consider commands old
+  std::chrono::milliseconds cmd_timeout_{500};
+  // save the timestamp when the last msg was received
+  rclcpp::Time last_msg_received_;
+
   // 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/src/joint_trajectory_controller.cpp

@@ -174,6 +174,16 @@ controller_interface::return_type JointTrajectoryController::update(
   // currently carrying out a trajectory
   if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())
   {
+    // is timeout configured?
+    if (params_.cmd_timeout > 0)
+    {
+      if (time - last_msg_received_ > cmd_timeout_)
+      {
+        RCLCPP_WARN(get_node()->get_logger(), "Aborted due to timeout");
+        set_hold_position();
+      }
+    }
+
     bool first_sample = false;
     // if sampling the first time, set the point before you sample
     if (!(*traj_point_active_ptr_)->is_sampled_already())
@@ -856,6 +866,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     std::string(get_node()->get_name()) + "/query_state",
     std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));
 
+  cmd_timeout_ = std::chrono::milliseconds{static_cast<int>(params_.cmd_timeout * 1000.0)};
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -1371,6 +1383,9 @@ bool JointTrajectoryController::validate_trajectory_msg(
 void JointTrajectoryController::add_new_trajectory_msg(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
 {
+  // we have to save this timestamp, because header timestamp can be zero by design
+  last_msg_received_ = this->get_node()->get_clock()->now();
+
   traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
 }
 

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -66,6 +66,11 @@ joint_trajectory_controller:
       one_of<>: [["splines", "none"]],
     }
   }
+  cmd_timeout: {
+    type: double,
+    default_value: 0.0, # seconds
+    description: "Timeout after which input command is considered staled. If zero, timeout is deactivated",
+  }
   gains:
     __map_joints:
       p: {

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -657,6 +657,104 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
   executor.cancel();
 }
 
+/**
+ * @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, true, {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(), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // 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 desired position sampled?
+  EXPECT_NEAR(state_msg->reference.positions[0], points.at(0).at(0), 1e-2);
+  EXPECT_NEAR(state_msg->reference.positions[1], points.at(0).at(1), 1e-2);
+  EXPECT_NEAR(state_msg->reference.positions[2], points.at(0).at(2), 1e-2);
+
+  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);
+  SetUpAndActivateTrajectoryController(executor, true, {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*
+
+  // auto clock = rclcpp::Clock(RCL_STEADY_TIME);
+  // rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(1.0);
+  publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // 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());
+
+  // TODO(christophfroehlich): using EXPECT_LT now ->
+  //  better calculate the true expected value after timeout
+  EXPECT_LT(
+    state_msg->reference.positions[0],
+    INITIAL_POS_JOINTS[0] + (points.at(0).at(0) - INITIAL_POS_JOINTS[0]) * cmd_timeout / 0.25);
+  EXPECT_LT(
+    state_msg->reference.positions[1],
+    INITIAL_POS_JOINTS[1] + (points.at(0).at(1) - INITIAL_POS_JOINTS[0]) * cmd_timeout / 0.25);
+  EXPECT_LT(
+    state_msg->reference.positions[2],
+    INITIAL_POS_JOINTS[2] + (points.at(0).at(2) - INITIAL_POS_JOINTS[0]) * cmd_timeout / 0.25);
+
+  executor.cancel();
+}
+
 /**
  * @brief check if position error of revolute joints are normalized if configured so
 */

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -363,7 +363,7 @@ class TrajectoryControllerTest : public ::testing::Test
     const auto end_time = start_time + wait_time;
     while (clock.now() < end_time)
     {
-      traj_controller_->update(clock.now(), clock.now() - start_time);
+      traj_controller_->update(node_->get_clock()->now(), clock.now() - start_time);
     }
   }