[JTC] Continue with last trajectory-point on success (ros-controls#842)

Author: christophfroehlich

joint_trajectory_controller/doc/userdoc.rst

@@ -150,12 +150,14 @@ Actions  [#f1]_
 <controller_name>/follow_joint_trajectory [control_msgs::action::FollowJointTrajectory]
   Action server for commanding the controller
 
-
 The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired.
+
 Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances.
 When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`).
 If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held.
 
+The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances.
+
 .. _Subscriber:
 
 Subscriber [#f1]_

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -174,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   // Timeout to consider commands old
   double cmd_timeout_;
-  // Are we holding position?
+  // True if holding position or repeating last trajectory point in case of success
   realtime_tools::RealtimeBuffer<bool> rt_is_holding_;
   // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
   bool subscriber_is_active_ = false;
@@ -244,9 +244,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void preempt_active_goal();
+
+  /** @brief set the current position with zero velocity and acceleration as new command
+   */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_hold_position();
 
+  /** @brief set last trajectory point to be repeated at success
+   *
+   * no matter if it has nonzero velocity or acceleration
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_success_trajectory_point();
+
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool reset();
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -362,7 +362,7 @@ controller_interface::return_type JointTrajectoryController::update(
             RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
 
             traj_msg_external_point_ptr_.reset();
-            traj_msg_external_point_ptr_.initRT(set_hold_position());
+            traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
           }
           else if (!within_goal_time)
           {
@@ -1466,6 +1466,20 @@ JointTrajectoryController::set_hold_position()
   return hold_position_msg_ptr_;
 }
 
+std::shared_ptr<trajectory_msgs::msg::JointTrajectory>
+JointTrajectoryController::set_success_trajectory_point()
+{
+  // set last command to be repeated at success, no matter if it has nonzero velocity or
+  // acceleration
+  hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back();
+  hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0);
+
+  // set flag, otherwise tolerances will be checked with success_trajectory_point too
+  rt_is_holding_.writeFromNonRT(true);
+
+  return hold_position_msg_ptr_;
+}
+
 bool JointTrajectoryController::contains_interface_type(
   const std::vector<std::string> & interface_type_list, const std::string & interface_type)
 {

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest
 
   void SetUpExecutor(
     const std::vector<rclcpp::Parameter> & parameters = {},
-    bool separate_cmd_and_state_values = false, double kp = 0.0)
+    bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0)
   {
     setup_executor_ = true;
 
-    SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp);
+    SetUpAndActivateTrajectoryController(
+      executor_, parameters, separate_cmd_and_state_values, kp, ff);
 
     SetUpActionClient();
 
@@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal)
 {
-  SetUpExecutor();
+  // deactivate velocity tolerance
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)};
+  SetUpExecutor(params, false, 1.0, 0.0);
   SetUpControllerHardware();
 
   std::shared_future<typename GoalHandle::SharedPtr> gh_future;
@@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa
     std::vector<JointTrajectoryPoint> points;
     JointTrajectoryPoint point;
     point.time_from_start = rclcpp::Duration::from_seconds(0.5);
-    point.positions.resize(joint_names_.size());
-
     point.positions = point_positions;
 
     points.push_back(point);
@@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
   // note: the action goal also is a trivial trajectory
-  if (traj_controller_->has_position_command_interface())
-  {
-    expectHoldingPoint(point_positions);
-  }
-  else
+  expectCommandPoint(point_positions);
+}
+
+TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal)
+{
+  // deactivate velocity tolerance and allow velocity at trajectory end
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0),
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpExecutor(params, false, 1.0, 0.0);
+  SetUpControllerHardware();
+
+  std::shared_future<typename GoalHandle::SharedPtr> gh_future;
+  // send goal
+  std::vector<double> point_positions{1.0, 2.0, 3.0};
+  std::vector<double> point_velocities{1.0, 1.0, 1.0};
   {
-    // no integration to position state interface from velocity/acceleration
-    expectHoldingPoint(INITIAL_POS_JOINTS);
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point;
+    point.time_from_start = rclcpp::Duration::from_seconds(0.5);
+    point.positions = point_positions;
+    point.velocities = point_velocities;
+
+    points.push_back(point);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
   }
+  controller_hw_thread_.join();
+
+  EXPECT_TRUE(gh_future.get());
+  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+
+  // run an update
+  updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
+
+  // it should be holding the last position goal
+  // i.e., active but trivial trajectory (one point only)
+  // note: the action goal also is a trivial trajectory
+  expectCommandPoint(point_positions, point_velocities);
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal)
 {
-  SetUpExecutor();
+  // deactivate velocity tolerance
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)};
+  SetUpExecutor({params}, false, 1.0, 0.0);
   SetUpControllerHardware();
 
   // add feedback
@@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal
     std::vector<JointTrajectoryPoint> points;
     JointTrajectoryPoint point1;
     point1.time_from_start = rclcpp::Duration::from_seconds(0.2);
-    point1.positions.resize(joint_names_.size());
-
     point1.positions = points_positions.at(0);
     points.push_back(point1);
 
     JointTrajectoryPoint point2;
     point2.time_from_start = rclcpp::Duration::from_seconds(0.3);
-    point2.positions.resize(joint_names_.size());
-
     point2.positions = points_positions.at(1);
     points.push_back(point2);
 
@@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal
 
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
-  if (traj_controller_->has_position_command_interface())
-  {
-    expectHoldingPoint(points_positions.at(1));
-  }
-  else
+  expectCommandPoint(points_positions.at(1));
+}
+
+TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal)
+{
+  // deactivate velocity tolerance and allow velocity at trajectory end
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0),
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  SetUpExecutor(params, false, 1.0, 0.0);
+  SetUpControllerHardware();
+
+  // add feedback
+  bool feedback_recv = false;
+  goal_options_.feedback_callback =
+    [&](
+      rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,
+      const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback>) { feedback_recv = true; };
+
+  std::shared_future<typename GoalHandle::SharedPtr> gh_future;
+  // send goal with multiple points
+  std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
+  std::vector<std::vector<double>> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}};
   {
-    // no integration to position state interface from velocity/acceleration
-    expectHoldingPoint(INITIAL_POS_JOINTS);
+    std::vector<JointTrajectoryPoint> points;
+    JointTrajectoryPoint point1;
+    point1.time_from_start = rclcpp::Duration::from_seconds(0.2);
+    point1.positions = points_positions.at(0);
+    point1.velocities = points_velocities.at(0);
+    points.push_back(point1);
+
+    JointTrajectoryPoint point2;
+    point2.time_from_start = rclcpp::Duration::from_seconds(0.3);
+    point2.positions = points_positions.at(1);
+    point2.velocities = points_velocities.at(1);
+    points.push_back(point2);
+
+    gh_future = sendActionGoal(points, 1.0, goal_options_);
   }
+  controller_hw_thread_.join();
+
+  EXPECT_TRUE(feedback_recv);
+  EXPECT_TRUE(gh_future.get());
+  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);
+
+  // run an update
+  updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
+
+  // it should be holding the last position goal
+  // i.e., active but trivial trajectory (one point only)
+  expectCommandPoint(points_positions.at(1), points_velocities.at(1));
 }
 
 /**
@@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)
 
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
-  expectHoldingPoint(points_positions.at(0));
+  expectCommandPoint(points_positions.at(0));
 }
 
 /**
@@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)
 
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
-  expectHoldingPoint(points_positions.at(1));
+  expectCommandPoint(points_positions.at(1));
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail)
@@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail)
 
   // it should be holding the position (being the initial one)
   // i.e., active but trivial trajectory (one point only)
-  std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
-  expectHoldingPoint(initial_positions);
+  expectCommandPoint(INITIAL_POS_JOINTS);
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
@@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
 
   // it should be holding the position (being the initial one)
   // i.e., active but trivial trajectory (one point only)
-  std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
-  expectHoldingPoint(initial_positions);
+  expectCommandPoint(INITIAL_POS_JOINTS);
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail)
@@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol
 
   // it should be holding the position (being the initial one)
   // i.e., active but trivial trajectory (one point only)
-  std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
-  expectHoldingPoint(initial_positions);
+  expectCommandPoint(INITIAL_POS_JOINTS);
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
@@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
 
   // it should be holding the last position,
   // i.e., active but trivial trajectory (one point only)
-  expectHoldingPoint(cancelled_position);
+  expectCommandPoint(cancelled_position);
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true)

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
   // it should still be holding the position at time of deactivation
   // i.e., active but trivial trajectory (one point only)
   traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
-  expectHoldingPoint(deactivated_positions);
+  expectCommandPoint(deactivated_positions);
 
   executor.cancel();
 }
@@ -459,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
   ASSERT_TRUE(traj_controller_->has_active_traj());
   // one point, being the position at startup
   std::vector<double> initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
-  expectHoldingPoint(initial_positions);
+  expectCommandPoint(initial_positions);
 
   executor.cancel();
 }
@@ -819,12 +819,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
   // should hold last position with zero velocity
   if (traj_controller_->has_position_command_interface())
   {
-    expectHoldingPoint(points.at(2));
+    expectCommandPoint(points.at(2));
   }
   else
   {
     // no integration to position state interface from velocity/acceleration
-    expectHoldingPoint(INITIAL_POS_JOINTS);
+    expectCommandPoint(INITIAL_POS_JOINTS);
   }
 
   executor.cancel();
@@ -1795,7 +1795,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
   updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
 
   // it should have aborted and be holding now
-  expectHoldingPoint(joint_state_pos_);
+  expectCommandPoint(joint_state_pos_);
 }
 
 TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
@@ -1827,14 +1827,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
   auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME));
 
   // it should have aborted and be holding now
-  expectHoldingPoint(joint_state_pos_);
+  expectCommandPoint(joint_state_pos_);
 
   // what happens if we wait longer but it harms the tolerance again?
   auto hold_position = joint_state_pos_;
   joint_state_pos_.at(0) = -3.3;
   updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time);
   // it should be still holding the old point
-  expectHoldingPoint(hold_position);
+  expectCommandPoint(hold_position);
 }
 
 // position controllers