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.

[fjonath1]

I don't know if you're still looking into this, but I actually have seemed to fix this, at least for a single robot setup.
Further modifications are necessary to accommodate multi-robot setup, since you need to figure out how to tie-in the input joint trajectory with the associated motion reply topic.

Basically the changes are:

• Create a topic that publish the joint_trajectory_streamer's MotionReplyResult. Also, for any validation
• Add joint_trajectory_action_v0, which is almost exact copy of joint_trajectory_action from ros_industrial with slight modification where we listen to the MotionReplyResult. This enables the action to preempt on validation error, abort on execution error, and also exits with success once the streamer state is back to IDLE and ready to execute the next trajectory.
• Add more tolerance into validation check, such that the joint trajectory streamer will replace the first trajectory point with the current joint state they are within 0.02rad difference. This helped with sending action back to back without getting rejection over the start of the trajectory. This was adapted from issue 324 comment, which seems to work pretty well together with the IDLE check I added on joint_trajectory_action_v0.

You can see the changeset in this fork.
With that fork, I can send back to back action without explicitly adding sleep in between the action call.