switch_controller timeout ignore sim_time

[cedricpradalier]

Describe the bug
controller_manager spawner fails when launched on a gazebo simulation started in pause mode.

To Reproduce
Steps to reproduce the behavior:

1. Starts a gazebo simulation in pause mode.
2. Starts a launch files that spawns a controller with use_sim_time =True
3. Wait for 5 second
4. Observe error message: [ruby $(which ign) gazebo-1] [ERROR] [.920830699] [a200_0000.controller_manager]: Switch controller timed out after 5.000000 seconds!

Alternatively, you can start the simulation running, or quickly press play, and the controller will load properly.

Expected behavior
If use_sim_time is true, the controller manager switching is expected to wait until the time is running. Alternatively, it should be possible to specify a workable timeout.

This problem stems from the combination of hard-coded timeout in controller_manager/spawner.py:

ret = switch_controllers(
                node, controller_manager_name, [], controller_names, True, True, 5.0
            )

And the wait on a std::condition_variable in controller_manager.cpp:

ros2_control/controller_manager/src/controller_manager.cpp

Line 1423 in d55def1

if (!switch_params_.cv.wait_for(

Given that there is no easy way to make a condition variable use sim time without redesigning this part of the code, the simplest alternative solution would be to export the timeout parameter in spawner.py, similarly to the controller_manager_timeout:

    parser.add_argument(
        "--controller-switch-timeout",
        help="Time to wait for the controller switch",
        required=False,
        default=5.0,
        type=float,
    )

In a simulation launch file for instance, I would set it to 60.0 to give enough time to the user to start the simulation.

[christophfroehlich]

duplicate of ros-controls/gz_ros2_control#421, "solved" with #1790

[christophfroehlich]

thanks for reporting, but I came up with a solution already. please give #1790 a try and leave your review there.

[cedricpradalier]

Thanks, this is indeed a duplicate. Sorry for not finding this reference before hand.