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.

[angel-ayala]

It makes sense, do you have any idea which part of the code should be modified for this to work? Or where could I start from to make this work?

[omichel]

I don't know exactly, but probably we should keep listening to the controllers while in pause mode and handle messages asking to resume the execution mode. The controller communication code is located here.

[angel-ayala]

Ok, I will take a look and try to continue from there.

[angel-ayala]

Seems more related to the Drive.cpp file where inside the loop wait ROS wait to finish all the tasks, and in the Python implementation, the ROS2 node's referenced is not passed.
It only passes a simple Python implementation of the WebotsNode.

I'm trying to pass the C++ object to the Python class, but with no luck.
There are some posts indicating a wrapper version to correctly do this, I just found SWIG and Boost.

What do you think? It may solves the problems?
It there any of these libraries implemented in the project?

[omichel]

No, I strongly recommend against SWIG and Boost for this purpose. Instead, you should probably use the Python/C native interface: https://docs.python.org/3/library/ctypes.html.

The ctypes module is part of the standard library, and therefore is more stable and widely available than swig, which always tended to give me problems.

[angel-ayala]

I was looking to create a correct wrapper to expose the C code and use it in Python to maintain one webots_ros2_driver::WebotsNode only in both C++ and python scope, but seems that requires to do the entire rclcpp::Node methods to use with ctypes in python.

I added the following code to PythonPlugin.cpp and can perform successfully inside python using ctypes casting.
Have you any other better idea? I was looking if any rclcpp or rclpy library already exposes some methods with no luck.

extern "C" {
  // Define a function to create a C++ object
  void* PythonPlugin_create_node(const char* robotName) {
    std::cout << "Hello, " << robotName << " from C++!" << std::endl;
    std::string nodeName = robotName;
    rclcpp::InitOptions options{};
  #if FOXY
    options.shutdown_on_sigint = false;
  #else
    options.shutdown_on_signal = false;
  #endif
    rclcpp::init(0, nullptr, options);
    for (char notAllowedChar : " -.)(")
      std::replace(nodeName.begin(), nodeName.end(), notAllowedChar, '_');
    mNode.reset(new WebotsNode(nodeName));
    std::cout << typeid(mNode).name() << std::endl;
    return reinterpret_cast<void*>(mNode.get());
  }
  void PythonPlugin_delete_node(void* node) {
    auto webots_node = reinterpret_cast<webots_ros2_driver::WebotsNode*>(node);
    delete webots_node;
  }
  // Define a function 
  const char* WebotsNode_get_name(void* node) {
    auto webots_node = static_cast<webots_ros2_driver::WebotsNode*>(node);
    std::string name_str = webots_node->get_name();
    const char* name_cstr = name_str.c_str();
    return name_cstr;
  }
  // Define a function 
  void WebotsNode_logger_info(void* node, const char* msg) {
    auto webots_node = static_cast<webots_ros2_driver::WebotsNode*>(node);
    RCLCPP_INFO(webots_node->get_logger(), msg);
  }
}

[abdulkadrtr]

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
...

@angel-ayala Hi, I want to run the webots reset functions that I can call with ros2 call .. in a webots simulation for an RL research project, but the code you gave gives an error. Can you give me information on how to import SetInt?

[ros2_supervisor.py-2] NameError: name 'SetInt' is not defined
[ERROR] [ros2_supervisor.py-2]: process has died [pid 14870, exit code 1, cmd '/home/abd/Desktop/mavic_ws/install/webots_ros2_driver/lib/webots_ros2_driver/ros2_supervisor.py --ros-args'].

[angel-ayala]

Hi @abdulkadrtr I actually implemented the SetInt ROS service in the webots_ros2_msg packages, you can take a look on the SetString service as example.

As mentioned before:

I don't know exactly, but probably we should keep listening to the controllers while in pause mode and handle messages asking to resume the execution mode. The controller communication code is located here.

The changes probably should be done on the webots repo, I don't know, I was with no time to dig deeper into this.