runtime/rbs_simulation/launch/simulation.launch.py

import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "rbs_robot_type",
            description="Type of robot by name",
            choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
            default_value="rbs_arm",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
    )
    declared_arguments.append(
        DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
    )
    
    sim_gazebo = LaunchConfiguration("sim_gazebo")
    rbs_robot_type = LaunchConfiguration("rbs_robot_type")
    env_manager_cond = LaunchConfiguration("env_manager")
    gazebo_gui = LaunchConfiguration("gazebo_gui")
    # FIXME: To args when we'll have different files
    # TODO: Use global variable to find world file in robossembler_db
    world_config_file = PathJoinSubstitution(
        [FindPackageShare("rbs_simulation"), "worlds", "asm2.sdf"]
    )
    # Gazebo nodes

    gazebo_server = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_gz_sim'),
                            'launch', 'gz_sim.launch.py')]),
            launch_arguments=[('gz_args', [' -r ',world_config_file, " -s"])],
            condition=UnlessCondition(gazebo_gui))
    
    gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_gz_sim'),
                            'launch', 'gz_sim.launch.py')]),
            launch_arguments=[('gz_args', [' -r ',world_config_file])],
            condition=IfCondition(gazebo_gui))
    
    # Spawn robot
    gazebo_spawn_robot = Node(package='ros_gz_sim', executable='create',
                 arguments=[
                    '-name', rbs_robot_type,
                    '-x', '0.0',
                    '-z', '0.0',
                    '-y', '0.0',
                    '-topic', '/robot_description'],
                 output='screen',
                 condition=IfCondition(sim_gazebo))
    env_manager = Node(package="env_manager", 
                       executable="run_env_manager",
                       condition=IfCondition(env_manager_cond)
                       )
    
    # Bridge
    rgbd_bridge_out = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/outer_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/outer_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
            '/outer_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
        ],
        output='screen',
        condition=IfCondition(sim_gazebo)
    )
    
    rgbd_bridge_in = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/inner_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/inner_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
            '/inner_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
        ],
        output='screen',
    )
    
    

    nodes_to_start = [
        gazebo,
        gazebo_server,
        gazebo_spawn_robot,
        env_manager,
        rgbd_bridge_out,
        rgbd_bridge_in
    ]
    return LaunchDescription(declared_arguments + nodes_to_start)