[JTC] Backport recent changes

.github/ISSUE_TEMPLATE/good-first-issue.md

@@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa
 
 - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along!
 
-- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling).
+- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://control.ros.org/humble/doc/getting_started/getting_started.html#building-from-source).
 
 - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork)
 
@@ -53,8 +53,8 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa
 
 Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below!
 Furthermore, you find helpful resources here:
-* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html)
-* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html)
+* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html)
+* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html)
 * [ROS Answers](https://answers.ros.org/questions/)
 
 **Good luck with your first issue!**

doc/writing_new_controller.rst

@@ -5,7 +5,7 @@
 Writing a new controller
 ========================
 
-In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib <https://docs.ros.org/en/{REPOS_FILE_BRANCH}/Tutorials/Beginner-Client-Libraries/Pluginlib.html>`_ interface.
+In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib <https://docs.ros.org/en/{DISTRO}/Tutorials/Beginner-Client-Libraries/Pluginlib.html>`_ interface.
 The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller.
 
 1. **Preparing package**

joint_trajectory_controller/CMakeLists.txt

@@ -58,8 +58,7 @@ if(BUILD_TESTING)
   target_link_libraries(test_trajectory joint_trajectory_controller)
 
   ament_add_gmock(test_trajectory_controller
-    test/test_trajectory_controller.cpp
-    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
+    test/test_trajectory_controller.cpp)
   set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
   target_link_libraries(test_trajectory_controller
     joint_trajectory_controller

joint_trajectory_controller/README.md

@@ -2,4 +2,4 @@
 
 The package implements controllers to interpolate joint's trajectory.
 
-For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/).
+For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/).

joint_trajectory_controller/doc/parameters.rst

@@ -62,11 +62,27 @@ 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.
 
   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.
 
@@ -77,6 +93,7 @@ constraints.stopped_velocity_tolerance (double)
 
 constraints.goal_time (double)
   Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time.
+  If set to zero, the controller will wait a potentially infinite amount of time.
 
   Default: 0.0 (not checked)
 

joint_trajectory_controller/doc/userdoc.rst

@@ -14,29 +14,40 @@ Waypoints consist of positions, and optionally velocities and accelerations.
 
 *Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license <https://creativecommons.org/licenses/by/3.0/>`_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_
 
-Hardware interface type [#f1]_
+Hardware interface types
 -------------------------------
 
-Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time:
+Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_) are supported in the following combinations as command interfaces:
+
+* ``position``
+* ``position``, ``velocity``
+* ``position``, ``velocity``, ``acceleration``
+* ``velocity``
+* ``effort``
+
+This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time:
 
 * For command interfaces ``position``, the desired positions are simply forwarded to the joints,
 * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints.
-* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`).
+* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`).
 
-This leads to the the following allowed combinations of command and state interfaces:
+This leads to the following allowed combinations of command and state interfaces:
 
 * With command interface ``position``, there are no restrictions for state interfaces.
 * With command interface ``velocity``:
 
   * if command interface ``velocity`` is the only one, state interfaces must include  ``position, velocity`` .
-  * no restrictions otherwise.
 
 * With command interface ``effort``, state interfaces must include  ``position, velocity``.
-* With command interface ``acceleration``, there are no restrictions for state interfaces.
 
-Example controller configurations can be found :ref:`below <ROS 2 interface>`.
+* With command interface ``acceleration``, state interfaces must include  ``position, velocity``.
+
+Further restrictions of state interfaces exist:
 
-Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands).
+* ``velocity`` state interface cannot be used if ``position`` interface  is missing.
+* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present."
+
+Example controller configurations can be found :ref:`below <ROS 2 interface>`.
 
 Other features
 --------------
@@ -105,7 +116,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
@@ -120,14 +130,8 @@ States
 ,,,,,,,,,,,,,,,,,,
 
 The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``.
-Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_.
-
-Legal combinations of state interfaces are:
 
-* ``position``
-* ``position`` and ``velocity``
-* ``position``, ``velocity`` and ``acceleration``
-* ``effort``
+Legal combinations of state interfaces are given in section `Hardware Interface Types`_.
 
 Commands
 ,,,,,,,,,
@@ -147,12 +151,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]_
@@ -162,7 +168,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

@@ -70,15 +70,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   JointTrajectoryController();
 
   /**
-   * @brief command_interface_configuration This controller requires the position command
-   * interfaces for the controlled joints
+   * @brief command_interface_configuration
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
 
   /**
-   * @brief command_interface_configuration This controller requires the position and velocity
-   * state interfaces for the controlled joints
+   * @brief command_interface_configuration
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
@@ -168,25 +166,28 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   std::vector<PidPtr> pids_;
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
   std::vector<double> ff_velocity_scale_;
-  // Configuration for every joint, if position error is normalized
-  std::vector<bool> normalize_joint_error_;
+  // Configuration for every joint, if position error is wrapped around
+  std::vector<bool> joints_angle_wraparound_;
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
+  // Timeout to consider commands old
+  double cmd_timeout_;
+  // 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;
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
     nullptr;
 
   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;
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
     traj_msg_external_point_ptr_;
 
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> hold_position_msg_ptr_ = nullptr;
+
   using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
   using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
   using StatePublisherPtr = std::unique_ptr<StatePublisher>;
@@ -205,6 +206,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);
 
@@ -246,20 +248,39 @@ 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
-  void set_hold_position();
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> set_success_trajectory_point();
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool reset();
 
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  bool has_active_trajectory() const;
+
   using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void publish_state(
     const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state,
     const JointTrajectoryPoint & state_error);
 
-  void read_state_from_hardware(JointTrajectoryPoint & state);
+  void read_state_from_state_interfaces(JointTrajectoryPoint & state);
 
+  /** Assign values from the command interfaces as state.
+   * This is only possible if command AND state interfaces exist for the same type,
+   *  therefore needs check for both.
+   * @param[out] state to be filled with values from command interfaces.
+   * @return true if all interfaces exists and contain non-NaN values, false otherwise.
+   */
   bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
   bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);
 
@@ -268,9 +289,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);
 
 private:
+  void update_pids();
+
   bool contains_interface_type(
     const std::vector<std::string> & interface_type_list, const std::string & interface_type);
 
+  void init_hold_position_msg();
   void resize_joint_trajectory_point(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
   void resize_joint_trajectory_point_command(

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -130,15 +130,15 @@ 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
   {
     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/include/joint_trajectory_controller/validate_jtc_parameters.hpp

@@ -42,7 +42,7 @@ tl::expected<void, std::string> command_interface_type_combinations(
   {
     return tl::make_unexpected(
       "'velocity' command interface can be used either alone or 'position' "
-      "interface has to be present");
+      "command interface has to be present");
   }
 
   if (
@@ -52,7 +52,7 @@ tl::expected<void, std::string> command_interface_type_combinations(
   {
     return tl::make_unexpected(
       "'acceleration' command interface can only be used if 'velocity' and "
-      "'position' interfaces are present");
+      "'position' command interfaces are present");
   }
 
   if (