Reset GUI Webots : webots_ros2 trayectory not responding

[LuisYordano]

Hi,

I am testing Webots with ROS2, the tutorial universal_robot:

Robot UR5e initial position:

I execute the trajectory:

ros2 action send_goal /follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{ trajectory: { joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint], points: [ { positions: [3.02, -1.63, -1.88, 1.01, 1.51, 1.13], velocities: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], accelerations: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], time_from_start: { sec: 5, nanosec: 500 } }, ] }, goal_tolerance: [ { name: shoulder_pan_joint, position: 0.01 }, { name: shoulder_lift_joint, position: 0.01 }, { name: elbow_joint, position: 0.01 }, { name: wrist_1_joint, position: 0.01 }, { name: wrist_2_joint, position: 0.01 }, { name: wrist_3_joint, position: 0.01 } ] }"

All correct:

I reset the simulation :

I execute the same trajectory :

The state is "Goal Accepted" in the action_server, but the trajectory never executed.

Thanks in advance for your answer.

[DavidMansolino]

Hi @LuisYordano, thank you for this precise bug report. I can indeed reproduce this.
In general, when using ROS2 you should not revert or reload the simulation as this will stop the ROS2 node communicating with Webots.
But there is indeed a bug, when the simulation is reverted the ROS2 node does not end correctly (which is why you did get the Goal Accepted), I am attempting to fix this in #39 (without success for now).

[LuisYordano]

Hi @DavidMansolino,

I don't know if this helps, but in Gazebo there is the commands:

- rosservice call /gazebo/reset_simulation "{}"

- rosservice call /gazebo/reset_world "{}"
 
- rosservice call /gazebo/pause_physics "{}"
 
- rosservice call /gazebo/unpause_physics "{}"

Webots is different from Gazebo, but the concepts of Reset Simulation, World, Pause, UnPause, would be implemented with the same philosophy in Webots as services in ROS2.

[DavidMansolino]

Thank you for the information.
We have indeed not yet implemented these functionalities, but we will shortly so that you can easily reset the world.
Just one small question, when you reset the world, the nodes remain connected to Gazebo, right?

[LuisYordano]

Affirmative!!!
Nodes only terminate communication with ROS Master (ROS1) and ROS2 when the user press CTRL+C.

[DavidMansolino]

Ok, thank you for the confirmation, I have added a new issue specific to these missing services: #40 (do not hesitate to comment if you think some other important services are missing).

[lukicdarkoo]

What about respawning the driver?

def generate_launch_description():
    webots_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        parameters=[
            {'robot_description': robot_description }
        ],

        # Every time one resets the simulation the controller is automatically respawned
        respawn=True
    )

    webots = WebotsLauncher(world=PathJoinSubstitution([package_dir, 'worlds', world]))

    return LaunchDescription([webots, webots_driver])

If you have some other nodes that have to be restarted (e.g. ros2_control) then you can use the OnProcessExit event handler:

def get_ros2_control_spawners(*args):
    return [
        Node(
            package='controller_manager',
            executable='spawner.py',
            arguments=['diffdrive_controller']
        )
    ]

def generate_launch_description():
    webots_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        parameters=[
            {'robot_description': robot_description }
        ],

        # Every time one resets the simulation the controller is automatically respawned
        respawn=True
    )

    webots = WebotsLauncher(world=PathJoinSubstitution([package_dir, 'worlds', world]))

    return LaunchDescription([
        webots,
        webots_driver,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=tiago_driver,
                on_exit=get_ros2_control_spawners,
            )
        )
    ] + get_ros2_control_spawners())

[lukicdarkoo]

I will close the issue. Feel free to re-open it if the answer doesn't solve your problem