Further Trajectory Completion Adjustments

[Jmeyer1292]

This PR follows up on #115 addressing #114.

While #115 removes the immediate check for goal completion, the action server can still terminate immediately upon the relieving its first feedback if a point toward the beginning of the trajectory is close to the end and the robot is slow to start.

This PR forces the action server to wait for half of the trajectories total duration before checking for the robot state against the end state. This gives time for the robot to start moving and publishing the in motion flag.

I have tested this pretty extensively in the Godel project and it seems to fix my issues. I am not sure if this is an appropriate solution, even as a temporary fix, but the issue remains and I thought I'd at least see what y'all think.

[gavanderhoorn]

This to me seems like a work-around: why start checking after half of the trajectory has been executed? Why not only check after the last point of it has been sent?

Would this solve the problem of trying to execute a 'figure eight' kind of trajectory for instance? Where the path of the TCP intersects with the starting point multiple times?

[shaun-edwards]

This to me seems like a work-around: why start checking after half of the trajectory has been executed? Why not only check after the last point of it has been sent?

Ideally this would be how the problem is solved. Unfortunately, the action interface does not know when the last point was sent. Only the robot client node does. A better solution may be to add the action interface to the client node, but this is a larger, more invasive change.

Would this solve the problem of trying to execute a 'figure eight' kind of trajectory for instance? Where the path of the TCP intersects with the starting point multiple times?

In this case, the inMotion status would prevent this confusion. This PR solve the problem where a path begins and ends at that same point, combined with a delay in the start of the motion such that the inMotion status is false and the current robot position is simultaneously at the start and end points.

[gavanderhoorn]

@shaun-edwards wrote:

This to me seems like a work-around: why start checking after half of the trajectory has been executed? Why not only check after the last point of it has been sent?

Ideally this would be how the problem is solved. Unfortunately, the action interface does not know when the last point was sent. Only the robot client node does. A better solution may be to add the action interface to the client node, but this is a larger, more invasive change.

Perhaps it would be an option to add a feedback signal between the action server and the robot client, in a similar way that actionlib uses *Feedback msgs to update its clients of status. It would make the bridging behaviour of the industrial_robot_client nodes more complete and would allow the action server to check for completion (actually only completion of the trajectory queueing, but that is a different issue).

Would this solve the problem of trying to execute a 'figure eight' kind of trajectory for instance? Where the path of the TCP intersects with the starting point multiple times?

In this case, the inMotion status would prevent this confusion. This PR solve the problem where a path begins and ends at that same point, combined with a delay in the start of the motion such that the inMotion status is false and the current robot position is simultaneously at the start and end points.

Then this is still a work-around: checking inMotion is clearly not enough to be able to derive what the state of the robot / trajectory is. Something like the EXECUTING state as proposed in ros-industrial/rep#12 would allow us to unambiguously determine server state (especially since the sending of the trajectory points is only meant to queue them, and acks on those msgs are actually not a confirmation of trajectory execution).

[gavanderhoorn]

Separating inMotion from something like EXECUTING also allows for trajectories in which there are segments with no motion. I can imagine welding would be one process where that would be a requirement.

[shaun-edwards]

All of these suggestions are good ones. I propose to accept this PR as is and log an issue for a better solution? Thoughts?

[gavanderhoorn]

Sure, we can merge this. At least as a workable work-around for now.

[shaun-edwards]

+1 for the work-around. Issue #114 was reopened to be addressed by a better, more appropriate fix later. Thanks @Jmeyer1292 for the contribution and all for the discussion.