Error handling of action server (joint_trajectory_action)

[akihikoy]

• ROS: ROS-1, melodic.
• motoman_driver: based on #70d731cb (I forked it and modified slightly).
• Robot: SG650, MotoMINI.
• Controller: YRC1000micro.

---

I'm working on a trajectory control of Motoman robots via joint_trajectory_action. The question I would like to raise here is about the error handling on the action client side.

In my understanding, the motoman_driver consists of:
[user node with action client] --(follow joint trajectory action)--> [joint_trajectory_action] --(joint path command)--> [motion_streaming_interface] --(simple_messages)--> [MotoPlus on Controller].

Sometimes I experience for example following errors:

1. [ERROR] [...]: Validation failed: Trajectory doesn't start at current position.
2. [ERROR] [...]: Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

The issue I want to discuss here is that when those errors happen, the action client (TSimpleActionClient class) cannot notice them. I tried to fix this issue by updating the motoman_driver code, but has noticed that there is a structural issue of the components.

The "Validation failed" error is checked in the [motion_streaming_interface] node (by the function MotomanJointTrajectoryStreamer::is_valid, used in trajectory_to_msgs).

The "Aborting Trajectory (3011)" error is checked in [MotoPlus on Controller] inside the Ros_MotionServer_JointTrajDataProcess function (MotionServer.c), and replied to the [motion_streaming_interface] node via Ros_SimpleMsg_MotionReply function.

So, at least those error states are recognized in the PC side, in the [motion_streaming_interface] node. However, currently there is no clear way to feedback those errors to the [joint_trajectory_action] node, and thus, the [joint_trajectory_action] node cannot reject (setRejected) or abort (setAborted) the active goal (active_goal_).

The [joint_trajectory_action] node is subscribing robot_status and feedback_status topics from the joint_state node, but I could not find adequate elements to put those error states.

So, in order to figure out the above issue, I'm planning to implement:

1. Updating [motion_streaming_interface] (the MotomanJointTrajectoryStreamer class) to publish the trajectory control errors.
2. Updating [joint_trajectory_action] (the JointTrajectoryAction class of ros-industrial/industrial_robot_client; as my setup is not multi-group) to subscribe the error feedback from [motion_streaming_interface] and reject or abort the active goal according to the errors.

Is this an adequate way? Do you come up with a better approach? Or did someone already try? Any advice is appreciated.

Many thanks for your support.