This pull request fixes two issues. First, the Rethink SDK continually issues the current command mode while running examples, in which case we only need to start/stop controllers when the mode changes, not every time the mode command is received. Also, when an example program like the wobbler.py is run, it sends back to back mode switch commands from position to velocity and then back again, one for right arm, and one for left, which was causing an error due to the ControllerManager::switchController method in ~ros_control/controller_manager/src/controller_manager.cpp not being re-entrant/thread safe.
This pull request fixes two issues. First, the Rethink SDK continually issues the current command mode while running examples, in which case we only need to start/stop controllers when the mode changes, not every time the mode command is received. Also, when an example program like the wobbler.py is run, it sends back to back mode switch commands from position to velocity and then back again, one for right arm, and one for left, which was causing an error due to the ControllerManager::switchController method in ~ros_control/controller_manager/src/controller_manager.cpp not being re-entrant/thread safe.