Multi-robot spawning in WeBots from launch

[willgraham345]

I've been having some issues with spawning Robots within my webots environment. My end goal is to spawn a few crazyflies and potentially a few turtlebots to an already running simulation, or use them as parameters to add to a simulation. I've been looking into the documentation extensivly regarding URDF's and Xacros. I wasn't able to find much regarding spawning robots via the Supervisor Node.

One launch file looks like this:

import os

import launch
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from webots_ros2_driver.webots_launcher import Ros2SupervisorLauncher,WebotsLauncher
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node

package_dir = get_package_share_directory('webots_pkg')


def generate_launch_description():
    """
    Launches the webots world and the ros2 supervisor
    """
    # THIS is for the robot_driver (unnecessary for webots_pkg)
    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'apartment.wbt')
    )
    ros2_supervisor = Ros2SupervisorLauncher()

    # Add event handler so everything shuts down on Webots exit
    event_handler = launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )


    return LaunchDescription([
        webots,
        ros2_supervisor,
        event_handler,
    ])


if __name__ == "__main__":
    generate_launch_description()
``` python

And the other looks like this:
``` python
#!/usr/bin/env python
# Launch the test locally: launch_test src/webots_pkg/test/test_webots_world_launch.py
import os
import rclpy
import pathlib
import launch
from launch import LaunchDescription
import launch.actions
import launch_testing
import launch_testing.actions
from launch_ros.actions import Node

from ament_index_python.packages import get_package_share_directory
from launch.substitutions.path_join_substitution import PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import IncludeLaunchDescription

from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node


def generate_launch_description():
    package_dir = get_package_share_directory('webots_pkg')
    urdf_path_cf = os.path.join(package_dir, 'resource', 'crazyflie.urdf')
    urdf_path_tb = os.path.join(package_dir, 'resource', 'turtlebot.urdf')


    robot_description_cf = pathlib.Path(urdf_path_cf).read_text()
    robot_description_tb = pathlib.Path(urdf_path_tb).read_text()

    spawn_URDF_cf = URDFSpawner(
        name = "cf1",
        urdf_path = urdf_path_cf,
        translation = '0 0 .015',
        rotation = '0 0 0 1',
    )

    driver_cf = Node(
        package = 'webots_ros2_driver',
        executable = 'driver',
        output = 'screen',
        additional_env = {'WEBOTS_ROBOT_NAME': 'cf1'},
        parameters = [{
            'use_sim_time': True,
            'robot_description': robot_description_cf,
        }],
    )


    return LaunchDescription([
        spawn_URDF_cf,
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessIO(
                target_action = spawn_URDF_cf,
                on_stdout=lambda event: get_webots_driver_node(event, driver_cf),
            )
        ),

        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=driver_cf,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        ),
    ])  

When I exit the second launch file, here is my output:

will@will:~/drl_swarm$ ros2 launch webots_pkg webots_spawner_launch.py 
[INFO] [launch]: All log files can be found below /home/will/.ros/log/2023-09-19-13-24-24-131954-will-41658
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2-1]: process started with pid [41660]
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
WARNING: the Ros2Supervisor was not able to spawn this URDF robot.
[ERROR] [ros2-1]: process has died [pid 41660, exit code 2, cmd 'ros2 service call /spawn_urdf_robot webots_ros2_msgs/srv/SpawnUrdfRobot {robot: {name: "cf1",urdf_path: "/home/will/drl_swarm/install/webots_pkg/share/webots_pkg/resource/crazyflie.urdf",translation: "0 0 .015",rotation: "0 0 0 1",} }'].

Any ideas on how to help with this problem

Also, here's my rqt graph if it is helpful

This was run on Ubuntu 20.04, and Webots 2022b with ROS Foxy. I can change that as needed.

[omichel]

I would recommend you to upgrade to Humble (or Iron) as Foxy is not supported any more.

[willgraham345]

I've now upgrade my ros installation to humble, and I'm running into a problem that there isn't a webots_ros2_tests module. Is that only available on Iron?

[willgraham345]

As some more background, here is the file I'm trying to run to get running:

#!/usr/bin/env python
import os
import rclpy
import pathlib
from ament_index_python.packages import get_package_share_directory
import launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.substitutions.path_join_substitution import PathJoinSubstitution
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node
from webots_ros2_driver.webots_launcher import WebotsLauncher


def generate_launch_description():
    package_dir = get_package_share_directory('webots_pkg')
    urdf_path_cf = os.path.join(package_dir, 'resource', 'crazyflie.urdf')
    urdf_path_tb = os.path.join(package_dir, 'resource', 'turtlebot.urdf')


    robot_description_cf = pathlib.Path(urdf_path_cf).read_text()
    robot_description_tb = pathlib.Path(urdf_path_tb).read_text()

    spawn_URDF_cf = URDFSpawner(
        name = "cf1",
        urdf_path = urdf_path_cf,
        translation = '-1 -1 .015',
        rotation = '0 0 0 1',
    )

    driver_cf = Node(
        package = 'webots_ros2_driver',
        executable = 'driver',
        output = 'screen',
        additional_env = {'WEBOTS_ROBOT_NAME': 'cf1'},
        parameters = [{
            'use_sim_time': True,
            'robot_description': robot_description_cf,
        }],
    )
    
    world = LaunchConfiguration('apartment.wbt')
    webots = WebotsLauncher(
        world=PathJoinSubstitution([package_dir, 'worlds', world]),
        ros2_supervisor=True,
    )


    return LaunchDescription([
        DeclareLaunchArgument(
            'apartment.wbt',
            default_value='apartment.wbt',
            description = 'Choose world file name from worlds folder'
        ),
        webots,

        webots._supervisor,

        spawn_URDF_cf,

        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessIO(
                target_action = spawn_URDF_cf,
                on_stdout=lambda event: get_webots_driver_node(event, driver_cf),
            )
        ),

        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=driver_cf,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        ),
    ])  

Here's what it looks like when launching:

[willgraham345]

Realized I had more going on under the surface with supervisor issues, and I needed to upgrade to Ros2 Humble. The below snippet essentially solved these problems, and now I'm onto different and hopefully easier bugs!

import os

import launch
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.webots_controller import WebotsController

package_dir = get_package_share_directory('webots_pkg')
def generate_launch_description():
    """
    Launches the webots world and the ros2 supervisor
    """
    # THIS is for the robot_driver (unnecessary for webots_pkg)
    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'apartment.wbt'),
        mode=''
    )

    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'apartment.wbt'),
        ros2_supervisor=True
    )
    # Add event handler so everything shuts down on Webots exit
    event_handler = launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )


    return LaunchDescription([
        webots,
        webots._supervisor,
        event_handler,
    ])


if __name__ == "__main__":
    generate_launch_description()