Fix pendant mode switch issue

[EricMarcil]

If the pendant mode switch is turned during the ROS motion the robot stops, but then when the playback is reenabled and the mode switch back to Remote mode, the robot moves because data in the motion queue was not cleared.

This PR fixes the issue #567 Unwanted motion when switching pendant mode

[ted-miller]

I'm not sure why this is necessary. Shouldn't this line get called when switching to play?
https://github.com/EricMarcil/motoman/blob/c2900ccffeaf953a2377b358d41ede5fb030a0c8/motoman_driver/MotoPlus/MotionServer.c#L1853

AFAICT, that's how MotoROS2 is handling the same situation.

[EricMarcil]

Not necessarily. It depends on the timing when the key switch is turned.
If the key is switch while the code is on the mpClkAnnounce
https://github.com/EricMarcil/motoman/blob/c2900ccffeaf953a2377b358d41ede5fb030a0c8/motoman_driver/MotoPlus/MotionServer.c#L1691
Then the Ros_Controller_IsMotionReady in the condition below is false and the code you are referring to is never reached.

New data coming from ROS get rejected elsewhere, but the motion in the queue ring buffer is not cleared.

So when the key is switched back to Remote and Ros_Controller_IsMotionReady becomes true, the remaining motion in the ring buffer gets executed.

If the key is switch while you are already passed that condition on line 1693, then the code you referred gets executed and there is not problem. You might want to restest a few times on MotoRos2.

[EricMarcil]

@ted-miller Can you also review and test on multi-robot system this PR when you get your Ros1 system running.

[ted-miller]

Let's hold off on incrementing the version number until #542 is also merged. I think both of these features should in the next version.
(Plus you forgot to increment APPLICATION_VERSION in MotoROS.h)

[ted-miller]

Can you also review and test on multi-robot system this PR when you get your Ros1 system running.

I was able to reproduce the issue using 1.9.11 and verify that this fixes it. This was on an R1+R2+S1 system.