Check to make sure that the goal has been reached

[kriskozak]

• Compare current state to goal state in execute() to make sure that the goal has actually been reached. This provides some extra assurance that that a failure to reach the goal will be detected. This addresses an issue that was identified with at least one robot/driver.
• Closeness to the goal is set by a reconfigurable parameter that was already present in robot/online_robot, and is used for an equivalent purpose.
• Add more detail to the log messages.

[shaun-edwards]

+1 from me.

[shaun-edwards]

@kriskozak, CI is failing now. It wasn't failing in the past. Have you verified that the tests passed on your local machine?

[kriskozak]

I can't seem to reproduce the unit test failure that is in the ci log. Here's where the test seems to fail on the CI server:

[moveit_simple.rosunit-kuka_kr210_utest/speed_reconfiguration][FAILURE]---------
/root/catkin_ws/src/moveit_simple/moveit_simple/test/kuka_kr210.cpp:468
Expected: robot->execute(TRAJECTORY_NAME) doesn't throw an exception.
  Actual: it throws.

When I run the tests locally, I see a failure (not every time, though) that is different, but I see it on both the PR branch and the target kinetic-devel branch. Here is where it fails locally:

[moveit_simple.rosunit-motoman_mh5_utest/speed_reconfiguration][FAILURE]--------
/kozak_ws/src/moveit_simple/moveit_simple/test/motoman_mh5.cpp:588
The difference between delta_time_for_speed_limits and 0.0 is 0.29256852822645385, which exceeds execution_time_tolerance, where
delta_time_for_speed_limits evaluates to -0.29256852822645385,
0.0 evaluates to 0, and
execution_time_tolerance evaluates to 0.25.

Note that the CI failure was for the kuka test, while the local failure was for the motoman test. Both failed the "speed reconfiguration" test, but I'm not sure whether that is a coincidence.

Actually, looking back through the logs, the kuka test did fail locally once (but that was before I got all the test dependencies installed, so other tests were also failing)...

After running it one more time I saw the kuka test fail in the same way as the motoman test:

[moveit_simple.rosunit-kuka_kr210_utest/speed_reconfiguration][FAILURE]---------
/kozak_ws/src/moveit_simple/moveit_simple/test/kuka_kr210.cpp:483
The difference between delta_time_for_speed_limits and 0.0 is 0.25862099645159042, which exceeds execution_time_tolerance, where
delta_time_for_speed_limits evaluates to -0.25862099645159042,
0.0 evaluates to 0, and
execution_time_tolerance evaluates to 0.25.

Given the fact that I'm not able to reliably reproduce the failure that was experienced on the CI, and I am seeing a possibly related error pop show up on both branches, I'm guessing that this may just be the result of one or more unreliable unit tests.

Perhaps try re-running it and see if it fails the same way again.