Buffered commands after safety node triggered

[tpatten]

If the arm stops moving because the safety node is triggered, the commands seem to buffer and are suddenly applied when the safety node is reset. Results in very quick and dangerous movements.

[tpatten]

@lokalmatador @smhaller

[ghost]

I'll fix that tomorrow to that part in that I can reset the joint_positioncommand to the actual joint states upon resetting safety. But if stuff remains inside a ros_control internal buffer - like the rest of the trajectory - I for now have no clue how to do that. Also I just realized I can't do it before Monday as Senka is using it tomorrow.

[ghost]

I added a fix that should resolve that issue. Engagement of the safetylock now is check not at the beginning of write but right before sending commands to any motors - this should guarantee that any internal ros_control buffer is emptied. Also, when releasing the lock, I just set the joint_positioncommand to the current jointposition. I can't test it before Monday, as Senka is using/calibrating/preparing the robot for pushing, maybe @tpatten can briefly have a look at it. If so, please check out my fork of squirrel_driver at https://github.com/lokalmatador/squirrel_driver/.

[tpatten]

We still do not have a working arm so I haven't been able to test. As soon as we fix our problem I'll take a look.

[tpatten]

I think this is fixed right? The unloading/loading solution.