Add Simulation Control Services

[DavidMansolino]

Similarly to Gazebo we should provide services to control the simulation, such as (http://gazebosim.org/tutorials/?tut=ros_comm):

• /webots/reset_simulation: should reset the simulation but not restart the controllers.
• /webots/reset_world: should reset only the robot position.
• /webots/pause_physics: should pause the simulation
• /webots/unpause_physics: should unpause the simulation
• /webots/spawn_model: should add an object to the root of the scene (with the possibility to set the translation/orientation (using a geometry_msgs/Pose initial_pose argument)
• /webots/delete_model: should remove an object from the root of the scene.

[WyattAutomation]

I'd be willing to pick this up if it's not stale (it can get approved soonish). Thanks ahead of time.

[lukicdarkoo]

If you need urgently these features you can extend the WebotsNode to the functionalities you need. The WebotsNode is designed to highly extendible, see an example here:
https://github.com/cyberbotics/webots_ros2/blob/master/webots_ros2_epuck/webots_ros2_epuck/driver.py#L64

[ygoumaz]

The Ros2_supervisor could be extended to create the missing services and apply them to the simulation.

[angel-ayala]

Hi, was adding the services and testing calling it directly from the ros console.
I started testing the pause and resume services, but only the pause service is working, the resume service present a bug.
After pausing, the resume service is unable to be executed, only after doing 2 simulation steps on the Webots GUI, the resume service is able to perform the coded action.
Seems that the spin() method of the inner webots ROS nodes are sync with wb_robot_step(), in this case should not works either way? Can you give me any guidance if this is ok or not?

Thanks,

Here are the ros2_supervisor.py additions:

...
class Ros2Supervisor(Node):
    def __init__(self):
        ...
        self.create_service(SetInt, 'reset_simulation', self.__reset_simulation_callback)
        self.create_service(SetInt, 'reset_world', self.__reset_world_callback)
        self.create_service(SetInt, 'pause_physics', self.__pause_physics_callback)
        self.create_service(SetInt, 'unpause_physics', self.__unpause_physics_callback)
        ...

    ...

    def __reset_simulation_callback(self, request, response):
        self.__robot.simulationReset()
        self.__robot.simulationResetPhysics()
        response.success = True
        return response

    def __reset_world_callback(self, request, response):
        self.get_logger().info("World has been reloaded.")
        self.__robot.worldReload()
        response.success = True
        return response

    def __pause_physics_callback(self, request, response):
        self.get_logger().info("Simulation has been paused.")
        self.__robot.simulationSetMode(self.__robot.SIMULATION_MODE_PAUSE)
        response.success = True
        return response

    def __unpause_physics_callback(self, request, response):
        self.get_logger().info("Simulation has been resumed.")
        self.__robot.simulationSetMode(self.__robot.SIMULATION_MODE_REAL_TIME)
        response.success = True
        return response
...

[omichel]

I believe it is currently not possible to resume a simulation after it was paused. This is because when Webots is in pause mode, it doesn't listen any more to the robot controllers. If we want to implement this resume-after-pause functionality, we should change Webots, so that it continues to listen to controller messages in pause mode.