Alternative implementation of multi-group trajectories and update motoman error handling

[nardavin]

Changes required to reliably control and execute joint trajectories on our multigroup system. This was tested and verified with a Yaskawa multi-group robot with one GP7 arm attached to a rail. This was not tested on a single-group robot to verify backwards compatibility - we suspect a small amount of work is still required to make these changes compatible with a single-group robot.

Changes were motivated by discussion in #450

Notable updates include:

• Add support for commanding multi-group robots through the joint trajectory action interface.
• Update the joint trajectory action server to only begin checking for successful trajectory end conditions after the trajectory is expected to end, in order to allow for trajectories with the same start and end position.
• Propagate motoros errors to the joint trajectory action node to abort the trajectory in case of motoros failure.
• Add a shutdown handler to verify the robot is disabled on exit.

[gavanderhoorn]

Thanks for the PR. It includes some interesting ideas.

c4533d3 seems to mimic ros-industrial/industrial_core#271, but makes it Motoman-specific.

d1cd81a is a nice idea, but on my test setup (YRC1000, 4 groups) results in all goals to always be ABORTED with:

[ WARN] [...] [/arm_controller/follow_joint_trajectory]: Outside goal constraints, aborting trajectory
[ INFO] [...] [/arm_controller/follow_joint_trajectory]: Aborting active goal.
[ INFO] [...] [/motion_streaming_interface]: Receiving joint trajectory message Dynamic
[ INFO] [...] [/motion_streaming_interface]: Empty trajectory received, canceling current trajectory
[ INFO] [...] [/move_group]: Controller 'arm_controller' successfully finished
[ WARN] [...] [/move_group]: Controller handle arm_controller reports status ABORTED
[ INFO] [...] [/move_group]: Completed trajectory execution with status ABORTED ...
[ INFO] [...] [/move_group]: Execution completed: ABORTED

f513b1e is also a nice idea. I'm wondering though, similar code is include here:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

Lines 192 to 196 in 7860ff5

	JointTrajectoryInterface::~JointTrajectoryInterface()
	{
	trajectoryStop();
	this->sub_joint_trajectory_.shutdown();
	}

I guess you observed that dtor to not always successfully disable trajectory mode?

[nardavin]

Thanks for the PR. It includes some interesting ideas.

c4533d3 seems to mimic ros-industrial/industrial_core#271, but makes it Motoman-specific.

d1cd81a is a nice idea, but on my test setup (YRC1000, 4 groups) results in all goals to always be ABORTED with:

[ WARN] [...] [/arm_controller/follow_joint_trajectory]: Outside goal constraints, aborting trajectory
[ INFO] [...] [/arm_controller/follow_joint_trajectory]: Aborting active goal.
[ INFO] [...] [/motion_streaming_interface]: Receiving joint trajectory message Dynamic
[ INFO] [...] [/motion_streaming_interface]: Empty trajectory received, canceling current trajectory
[ INFO] [...] [/move_group]: Controller 'arm_controller' successfully finished
[ WARN] [...] [/move_group]: Controller handle arm_controller reports status ABORTED
[ INFO] [...] [/move_group]: Completed trajectory execution with status ABORTED ...
[ INFO] [...] [/move_group]: Execution completed: ABORTED

I suspect something more subtle is going on here. Do the trajectories succeed on 01146c8? Not sure how adding the timer constraint would cause the trajectories to be aborted.

f513b1e is also a nice idea. I'm wondering though, similar code is include here:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

Lines 192 to 196 in 7860ff5

	JointTrajectoryInterface::~JointTrajectoryInterface()
	{
	trajectoryStop();
	this->sub_joint_trajectory_.shutdown();
	}

I guess you observed that dtor to not always successfully disable trajectory mode?

Correct - I'm not sure what was going on here, but adding the explicit sigterm handler was more reliable in shutting down the arm after killing the rosnode.

[akashjinandra]

This has been tested on our setup on a YRC1000 with 4 robot groups. Thanks for all your hard work!

[rsinnet]

This has been tested on our setup on a YRC1000 with 4 robot groups. Thanks for all your hard work!

Wow, thanks for the hardware testing!