ros_control upgrade breaks baxter_gazebo simulation

[frog0705]

Today, May 21 (China Time) there were numerous updates for ROS. After updating these I was no longer able to move Baxter in Simulation. Apparently there is an issue with ros_control.

It seems I upgraded from version 0.9.2 to 0.9.3

    ii  ros-indigo-control-msgs                                     1.3.1-0trusty-20150424-0624-+0000                   amd64        control_msgs contains base messages and actions useful for controlling robots. It provides representations for controller setpoints and joint and cartesian trajectories.
    ii  ros-indigo-control-toolbox                                  1.13.1-0trusty-20150430-2231-+0000                  amd64        The control toolbox contains modules that are useful across all controllers.
    ii  ros-indigo-controller-interface                             0.9.3-0trusty-20150507-1046-+0000                   amd64        Interface base class for controllers
    ii  ros-indigo-controller-manager                               0.9.3-0trusty-20150507-1051-+0000                   amd64        The controller manager.
    ii  ros-indigo-controller-manager-msgs                          0.9.3-0trusty-20150507-0820-+0000                   amd64        Messages and services for the controller manager.
    ii  ros-indigo-controller-manager-tests                         0.9.3-0trusty-20150507-1259-+0000                   amd64        controller_manager_tests

When running roslaunch baxter_gazebo baxter_world.launch the simulation loads but the arms are not responsive.

Then, when you try to move the arms using any baxter_example script, an error message shows up in the simulator terminal saying:

    [ INFO] [1432181335.517228641, 34.010000000]: Robot is disabled
    [ INFO] [1432181335.517704827, 34.010000000]: Gravity compensation was turned off
    [ERROR] [1432181351.136165406, 49.584000000]: Could not stop controller 'right_joint_velocity_controller' since it is not running
    [ERROR] [1432181351.136278313, 49.584000000]: Failed to switch controllers

Is anyone else experiencing this behavior?

[rethink-imcmahon]

Thanks for reporting this @frog0705. Now we need to narrow down the bug. Did ros_control break API in their Indigo patch or did this update just uncover an underlying bug? I will look into this a bit today, but any additional information you could provide would be very welcome.

[rethink-imcmahon]

Just a quick update. I think I have found the issue. ros_control has accepted a pull request this sync that changes the behavior of how controllers are switched: https://github.com/ros-controls/ros_control/pull/200

Previous to this PR, one could stop controllers whether or not they were started (which is the way we implemented baxter_gazebo). It appears now that this throws an error when controllers are already stopped. Our offending piece of code is contained in this switch case in baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp: https://github.com/RethinkRobotics/baxter_simulator/blob/master/baxter_gazebo/src/baxter_gazebo_ros_control_plugin.cpp#L207-L234

[rethink-imcmahon]

Ok, this line in baxter_gazebo_ros_control_plugin.cpp was a timebomb: controller_manager_msgs::SwitchController::Request::STRICT

Passing in a value of STRICT to the controller manager didn't matter before because it was never properly implemented. According to the service Request definition:

#    * STRICT means that switching will fail if anything goes wrong (an invalid
#      controller name, a controller that failed to start, etc. )

So it was only a matter of time before the functionality was fixed to the intended behavior. Unfortunately, we were relying on missing functionality. It does seem harsh for the controller to fail to switch because already-stopped controllers were passed in to stop, but I suppose that's why it's called STRICT.

[rethink-imcmahon]

I can verify that changing line 228 in baxter_gazebo_ros_control_plugin.cpp to controller_manager_msgs::SwitchController::Request::BEST_EFFORT solves the issue. The question now is, do we want to rely on BEST_EFFORT to be lenient or continue to use STRICT and do additional checking to verify the proper controllers have been identified for start/stop?

[rethink-imcmahon]

I think going with BEST_EFFORT is a good short-term fix for all until this issue can be revisited more thoroughly in the development cycle. I will add this change to a branch off master and pull request.