JointTrajectoryController fails to hold position on effort control

[c-rizz]

JointTrajectoryController fails to hold position when used with only an effort command interface. I believe this is because when the controller aborts because of a tolerance violation, it sets an empty goal and stops updating the joint_interfaces. In position control this means the position is kept constant, but not when using effort control.
In fact in my gazebo simulation the robot falls on itself when it hits a tolerance violation.

This could be fixed by having the update loop set the trajectory message to the current position in case of an abort, the PID should then manage to keep the position. Could this be done in all the cases in which the goal trajectory is empty? What is the intended behavior when the trajectory is empty?

[c-rizz]

I implemented a fix at https://github.com/c-rizz/ros2_controllers/tree/humble. For now I did it on humble, but I verified the problem is also present on master. With the current fix the controller keeps the position correctly in my use case. Also it now keeps the position on startup, I am not sure if that is desirable.
If it is fine I could submit a PR.

[christophfroehlich]

The same applies for canceling goals while using velocity command interface. Your patch solved this also for me, thanks!!
I also like the idea of automatically holding the start position, I solved that now always with sending a goal manually at startup.

I was investigating this issue because I also needed a solution for that -> I've allowed myself to cherrypick your commit and open a PR for the master branch. We then can backport it to humble.

[christophfroehlich]

Edit: now I realized the already existing PR

[christophfroehlich]

Fixed with #558