Error handling of action server (joint_trajectory_action)
akihikoy opened this issue · comments
- 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:
- [ERROR] [...]: Validation failed: Trajectory doesn't start at current position.
- [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:
- Updating [motion_streaming_interface] (the
MotomanJointTrajectoryStreamer
class) to publish the trajectory control errors. - 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.