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

I'll fix that tomorrow to that part in that I can reset the joint_position_command_ 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.

I added a fix that should resolve that issue. Engagement of the safety_lock_ 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_position_command_ to the current joint_position_. 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.