Rapid driver should validate initial conditions of trajectory before execution

[yamokosk]

My proposal is to add a a new function to the ROS_motion.mod task that would check the first point of the trajectory against the current state of the robot (essentially reuse the is_near function). If the first point is outside some tolerance of the current state, an error message would be written to a log (using ErrWrite) and the trajectory would be aborted (abort_trajectory).

Currently if the first point of the trajectory fails the is_near check, a slow (10s duration) movement is executed to the first point of the trajectory. If the robot's current state is only slightly out of tolerance the robot motion is imperceptible and it appears as if there is just a long time delay associated with sending trajectories to the robot (see ros-industrial/abb#142 for some history).

[gavanderhoorn]

There are two approaches here that could be considered:

1. perform the check for start_state~=current_state whilst queueing a new trajectory
2. perform the check at the start of execution of a new trajectory

The first option would be efficient, as it would immediately fail an upload and could potentially save some time. It would be a divergence from other drivers though, as they don't check incoming trajectory points, but rather fail during execution (which is then communicated using RobotStatus msgs).

The second option would be more in-line with what other drivers do, but has the disadvantage that for long trajectories there can be some time between uploading the trajectory and faulting it.

For option 1, ros-industrial/industrial_core#118 should probably be addressed: there is little point in erroring on the robot controller only with the ROS side not being updated on the failure. For option 2, ros-industrial/industrial_core#131 is probably important.

[davetcoleman]

+1 to failing rather than silent, slow, unexpected behavior.

@gavanderhoorn's two options are I believe debating between having the check on the Linux client side versus the ABB rapid code side. Performing the check in the rapid code to me seems to reduce the chance of false-positives, since it should in theory have a more current "current state" to check against.

[gavanderhoorn]

No, both would be on the controller.

The first option already checks upon reception of the first point whether it deviates. If it does, fail the upload.

The second allows the upload of the entire trajectory to complete (ie: succeed) and then fails if the first pt doesn't coincide with the current state.

According to the spec, industrial_robot_client compatible drivers should do the second. Execution should fail -- and be reported via the RobotStatus msgs preferably. The fact that a driver is an uploading or streaming driver shouldn't matter.

For efficiency reasons failing the upload could be nicer though: uploads don't take long, but I like a fail-early approach.

[gavanderhoorn]

Have you looked into implementing this @yamokosk?

[gavanderhoorn]

I've spent some time on this: https://github.com/ros-industrial/abb/compare/kinetic-devel...gavanderhoorn:check_traj_start.

Problem is that there is currently no way to notify the ROS side that we've dropped the trajectory, so the goal will never complete.

(we should of course actually cancel the goal, or fail it, but that is also not possible: https://github.com/ros-industrial/industrial_core/issues/118 and https://github.com/ros-industrial/industrial_core/issues/131)