[JTC] Explicitly set hold position (backport ros-controls#558)

joint_trajectory_controller/doc/parameters.rst

@@ -62,6 +62,12 @@ open_loop_control (boolean)
 
   Default: false
 
+start_with_holding (bool)
+  If true, start with holding position after activation. Otherwise, no command will be sent until
+  the first trajectory is received.
+
+  Default: true
+
 allow_nonzero_velocity_at_trajectory_end (boolean)
   If false, the last velocity point has to be zero or the goal will be rejected.
 

joint_trajectory_controller/doc/userdoc.rst

@@ -105,7 +105,6 @@ When an active action goal is preempted by another command coming from the actio
 
 Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action.
 
-
 .. _ROS 2 interface:
 
 Description of controller's interfaces
@@ -162,7 +161,7 @@ Subscriber [#f1]_
   Topic for commanding the controller
 
 The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring.
-The controller's path and goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations.
+The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held.
 Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface.
 
 

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -180,7 +180,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   rclcpp::Service<control_msgs::srv::QueryTrajectoryState>::SharedPtr query_state_srv_;
 
-  std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
   std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
   std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
@@ -205,6 +204,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
   RealtimeGoalHandleBuffer rt_active_goal_;  ///< Currently active action goal, if any.
+  realtime_tools::RealtimeBuffer<bool> rt_has_pending_goal_;  ///< Is there a pending action goal?
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
 
@@ -247,11 +247,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void preempt_active_goal();
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void set_hold_position();
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_hold_position();
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool reset();
 
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  bool has_active_trajectory();
+
   using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void publish_state(

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -130,6 +130,9 @@ class Trajectory
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool has_trajectory_msg() const;
 
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  bool has_nontrivial_msg() const;
+
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg() const
   {