Finished the passthrough controller

The number of joints in the controller is variable and received from the hardware interface. (It is not variable there though).
The internal action server uses the action type FollowJointTrajectory. Both goal time tolerance and goal position tolerance are checked upon completion of trajectory,
and feedback is sent to the user.
The integration test has also been updated to test the controller.

Author: URJala

examples/example_movej.py

@@ -29,7 +29,8 @@
 
 import rclpy
 from builtin_interfaces.msg import Duration
-from control_msgs.action import JointTrajectory
+from control_msgs.action import FollowJointTrajectory
+from control_msgs.msg import JointTolerance
 
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -100,7 +101,7 @@ def init_robot(self):
         self.jtc_action_client = waitForAction(
             self.node,
             "/passthrough_trajectory_controller/forward_joint_trajectory",
-            JointTrajectory,
+            FollowJointTrajectory,
         )
         time.sleep(2)
 
@@ -127,14 +128,23 @@ def send_trajectory(self, waypts, time_vec):
             point.time_from_start = time_vec[i]
             joint_trajectory.points.append(point)
 
+        tolerances = [JointTolerance(position=0.001) for i in range(6)]
+        time_tolerance = Duration()
+        time_tolerance.sec = 1
         # Sending trajectory goal
         goal_response = self.call_action(
-            self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory)
+            self.jtc_action_client,
+            FollowJointTrajectory.Goal(
+                trajectory=joint_trajectory,
+                goal_tolerance=tolerances,
+                goal_time_tolerance=time_tolerance,
+            ),
         )
         if goal_response.accepted is False:
             raise Exception("trajectory was not accepted")
 
         # Verify execution
+
         result = self.get_result(self.jtc_action_client, goal_response)
         return result
 
@@ -217,7 +227,7 @@ def load_passthrough_controller(self):
             self.jtc_action_client = waitForAction(
                 self.node,
                 "/passthrough_trajectory_controller/forward_joint_trajectory",
-                JointTrajectory,
+                FollowJointTrajectory,
             )
             time.sleep(2)
 
@@ -242,9 +252,9 @@ def switch_controller(self, active, inactive):
 
     # The following list are arbitrary joint positions, change according to your own needs
     waypts = [
-        [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406],
-        [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406],
-        [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406],
+        [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+        [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
+        [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349],
     ]
     time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
 

examples/example_spline.py

@@ -29,7 +29,7 @@
 
 import rclpy
 from builtin_interfaces.msg import Duration
-from control_msgs.action import JointTrajectory
+from control_msgs.action import FollowJointTrajectory
 
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -100,7 +100,7 @@ def init_robot(self):
         self.jtc_action_client = waitForAction(
             self.node,
             "/passthrough_trajectory_controller/forward_joint_trajectory",
-            JointTrajectory,
+            FollowJointTrajectory,
         )
         time.sleep(2)
 
@@ -131,7 +131,7 @@ def send_trajectory(self, waypts, time_vec, vels, accels):
 
         # Sending trajectory goal
         goal_response = self.call_action(
-            self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory)
+            self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
         )
         if goal_response.accepted is False:
             raise Exception("trajectory was not accepted")
@@ -219,7 +219,7 @@ def load_passthrough_controller(self):
             self.jtc_action_client = waitForAction(
                 self.node,
                 "/passthrough_trajectory_controller/forward_joint_trajectory",
-                JointTrajectory,
+                FollowJointTrajectory,
             )
             time.sleep(2)
 

examples/examples.py

@@ -29,7 +29,7 @@
 
 import rclpy
 from builtin_interfaces.msg import Duration
-from control_msgs.action import JointTrajectory
+from control_msgs.action import FollowJointTrajectory
 
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec):
 
         # Sending trajectory goal
         goal_response = self.call_action(
-            self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory)
+            self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
         )
         if goal_response.accepted is False:
             raise Exception("trajectory was not accepted")
@@ -211,7 +211,7 @@ def load_passthrough_controller(self):
             self.jtc_action_client = waitForAction(
                 self.node,
                 "/passthrough_trajectory_controller/forward_joint_trajectory",
-                JointTrajectory,
+                FollowJointTrajectory,
             )
             time.sleep(2)
 

examples/load_switch_controller_example.py

@@ -29,7 +29,7 @@
 
 import rclpy
 from builtin_interfaces.msg import Duration
-from control_msgs.action import JointTrajectory
+from control_msgs.action import FollowJointTrajectory
 
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec):
 
         # Sending trajectory goal
         goal_response = self.call_action(
-            self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory)
+            self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
         )
         if goal_response.accepted is False:
             raise Exception("trajectory was not accepted")
@@ -211,7 +211,7 @@ def load_passthrough_controller(self):
             self.jtc_action_client = waitForAction(
                 self.node,
                 "/passthrough_trajectory_controller/forward_joint_trajectory",
-                JointTrajectory,
+                FollowJointTrajectory,
             )
             time.sleep(2)
 

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -51,6 +51,8 @@
 #include "rclcpp_action/create_server.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/server_goal_handle.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/duration.hpp"
 
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
@@ -63,15 +65,36 @@ namespace ur_controllers
 {
 enum CommandInterfaces
 {
+  /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in.
+  0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory.
+  1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a
+  point to the hardware interface.
+  2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0
+  and 2.0 until all points have been read by the hardware interface.
+  3.0: The hardware interface has read all the points, and will now write all the points to the robot driver.
+  4.0: The robot is moving through the trajectory.
+  5.0: The robot finished executing the trajectory. */
   PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0,
-  PASSTHROUGH_POINT_WRITTEN = 1,
-  PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2,
-  PASSTHROUGH_TRAJECTORY_CANCEL = 3,
-  PASSTHROUGH_TRAJECTORY_DIMENSIONS = 4,
-  PASSTHROUGH_TRAJECTORY_POSITIONS_ = 5,
-  PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 11,
-  PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 17,
-  PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 23
+  /* The PASSTHROUGH_TRAJECTORY_CANCEL value is used to indicate whether the trajectory has been cancelled from the
+   * hardware interface.*/
+  PASSTHROUGH_TRAJECTORY_CANCEL = 1,
+  /* The PASSTHROUGH_TRAJECTORY_DIMENSIONS is used to indicate what combination of joint positions, velocities and
+   * accelerations the trajectory consists of, see check_dimensions() for a description of what the values mean. */
+  PASSTHROUGH_TRAJECTORY_DIMENSIONS = 2,
+  /* Arrays to hold the values of a point. */
+  PASSTHROUGH_TRAJECTORY_POSITIONS_ = 3,
+  PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 9,
+  PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 15,
+  PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 21
+};
+
+enum StateInterfaces
+{
+  /* State interface 0 - 17 are joint state interfaces*/
+
+  SPEED_SCALING_FACTOR = 18,
+  NUMBER_OF_JOINTS = 19,
+  CONTROLLER_RUNNING = 20
 };
 
 class PassthroughTrajectoryController : public controller_interface::ControllerInterface
@@ -93,32 +116,64 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
 private:
+  /* Start an action server with an action called: /passthrough_trajectory_controller/forward_joint_trajectory. */
   void start_action_server(void);
 
+  void end_goal();
+
+  bool check_goal_tolerance();
+
   std::shared_ptr<passthrough_trajectory_controller::ParamListener> passthrough_param_listener_;
   passthrough_trajectory_controller::Params passthrough_params_;
 
-  rclcpp_action::Server<control_msgs::action::JointTrajectory>::SharedPtr send_trajectory_action_server_;
+  rclcpp_action::Server<control_msgs::action::FollowJointTrajectory>::SharedPtr send_trajectory_action_server_;
 
-  rclcpp_action::GoalResponse goal_received_callback(
-      const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const control_msgs::action::JointTrajectory::Goal> goal);
+  rclcpp_action::GoalResponse
+  goal_received_callback(const rclcpp_action::GoalUUID& uuid,
+                         std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
 
   rclcpp_action::CancelResponse goal_cancelled_callback(
-      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::JointTrajectory>> goal_handle);
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
 
   void goal_accepted_callback(
-      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::JointTrajectory>> goal_handle);
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
+
+  void execute(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> goal_handle);
+
+  std::vector<std::string> joint_names_;
+  std::vector<std::string> state_interface_types_;
+
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_position_state_interface_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> joint_velocity_state_interface_;
+
+  /*
+  If there are values in the velocities and accelerations vectors, they should have as many elements as there are
+  joints, and all be the same size.
+  The return value indicates what combination of joint positions, velocities and accelerations is present;
+  0: The trajectory is invalid, and will be rejected.
+  1: Only positions are defined for the trajectory.
+  2: Positions and velocities are defined for the trajectory.
+  3: Positions and accelerations are defined for the trajectory.
+  4: Both positions, velocities and accelerations are defined for the trajectory.
+  */
+  int check_dimensions(std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Goal> goal);
 
-  void
-  execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::JointTrajectory>> goal_handle);
-  int check_dimensions(std::shared_ptr<const control_msgs::action::JointTrajectory::Goal> goal);
   trajectory_msgs::msg::JointTrajectory active_joint_traj_;
+  std::vector<control_msgs::msg::JointTolerance> path_tolerance_;
+  std::vector<control_msgs::msg::JointTolerance> goal_tolerance_;
+  rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0);
+
   uint32_t current_point_;
   bool trajectory_active_;
-  uint64_t active_trajectory_elapsed_time_;
-  uint64_t max_trajectory_time_;
+  uint64_t period_ns;
+  uint64_t last_time_ns;
+  uint64_t now_ns;
+  double active_trajectory_elapsed_time_;
+  double max_trajectory_time_;
   double scaling_factor_;
-  std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::JointTrajectory>> current_handle;
+  uint32_t number_of_joints_;
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>> active_goal_;
 };
 }  // namespace ur_controllers
 #endif  // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_