Hold joint position if tolerances were violated

[Martin-Oehler]

Currently, the joint_trajectory_controller keeps executing a trajectory even after the goal was aborted due to a tolerance violation. In my opinion, this is a bad idea for multiple reasons:

• It is unsafe to keep moving, since the robot deviated from the path. The chance of a collision is high.
• The current behavior is unexpected.
• Even if you wanted to, you could not stop the execution anymore, because an aborted goal can't be preempted again.

This behavior was mentioned in issue #48, but never fixed.

This PR addresses this issue by calling setHoldPosition() before aborting the goal.

I think, the current tests should be sufficient. However, I was unable to execute them since I got a lot of unrelated errors.

[mathias-luedtke]

However, I was unable to execute them since I got a lot of unrelated errors.

Please elaborate, this should not happen. The joint_trajectory_controller are a little bit unstable, but it is still possible to execute them.

[Martin-Oehler]

Your code passes rt_active_goal_ and therefore might prevent these problems of other fixes. However, I am not 100% sure of the implications. It looks like that this pointer is only used for the tolerance checks.

I can't really comment on that. I did not read too much into the code.

The current, but broken behavior was around for a long time, so I would suggest to not merge into kinetic unless we have a good reason for it.

Which release do you think I should target? Currently, I have only a running kinetic install.

No, the tests have to be extended to check that the robot really stopped. The test suite should fail before and pass after applying the patch.

Do you have input for that? In the current formulation of the tests, it is pretty much unknown, when and where the path tolerance violation happens. Is a test, if the joints are moving at all, sufficient?

Please elaborate, this should not happen.

I am executing

$ catkin build joint_trajectory_controller --catkin-make-args tests
$ rostest joint_trajectory_controller joint_trajectory_controller.test 

and receive

[INFO] [1545399417.555784] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1545399417.557350] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1545399417.558517] [/controller_spawner]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1545399417.559680] [/controller_spawner]: Loading controller: rrbot_controller
[INFO] [1545399417.590825] [/controller_spawner]: Controller Spawner: Loaded controllers: rrbot_controller
[INFO] [1545399417.600804] [/controller_spawner]: Started controllers: rrbot_controller
[ERROR] [1545399418.488635752] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399418.498615822] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399418.509027322] [/rrbot]: Size mismatch in trajectory point position, velocity or acceleration data.
[ERROR] [1545399418.519044854] [/rrbot]: Size mismatch in trajectory point position, velocity or acceleration data.
[ERROR] [1545399418.529086941] [/rrbot]: Trajectory message contains waypoints that are not strictly increasing in time.
[ERROR] [1545399418.539248713] [/rrbot]: Joints on incoming goal don't match the controller joints.
[ERROR] [1545399482.330258427] [/rrbot]: Unexpected exception caught when initializing trajectory from ROS message data.
[INFO] [1545399495.982264] [/controller_spawner]: Shutting down spawner. Stopping and unloading controllers...
[INFO] [1545399495.982821] [/controller_spawner]: Stopping all controllers...
[WARN] [1545399496.061524] [/controller_spawner]: Controller Spawner error while taking down controllers: unable to connect to service: [Errno 104] Connection reset by peer

However, I just noticed that after a few minutes, I still get

SUMMARY
 * RESULT: SUCCESS
 * TESTS: 25
 * ERRORS: 0
 * FAILURES: 0

[Martin-Oehler]

No, the tests have to be extended to check that the robot really stopped. The test suite should fail before and pass after applying the patch.

I added the respective test cases.

[Martin-Oehler]

Can I have an update on this? Are more changes requested?

[mathias-luedtke]

Thanks for your patches and your ping :)

I added the respective test cases.

It fails on travis, I think it needs some updates (see other comments).

Which release do you think I should target? Currently, I have only a running kinetic install.

I would target melodic (our current development branch). We can think about a back-port though.

[martiniil]

Hi.

I think, the current tests should be sufficient. However, I was unable to execute them since I got a lot of unrelated errors.

I tried to make some progress here. See #457. Currently, only the tolerance-tests fail. They should pass once this issue is solved. So I would like to comment them out and get the changes into melodic quickly. Then we should uncomment them here. Also makes it more convenient to add new test cases. @ipa-mdl What do you think?