simulation.launch

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node

def generate_launch_description():
    # Define paths
    bringup_dir = get_package_share_directory('nav2_bringup')
    world_file = os.path.join(bringup_dir, 'worlds', 'empty_ground.world') # EMPTY WORLD
    #world_file = os.path.join(bringup_dir, 'worlds', 'raceway.world') #SONOMA Raceway world # Friction is too high truck does not move
    rviz_config_file = os.path.join(bringup_dir, 'rviz', 'truck.rviz')
    truck_urdf_file = os.path.join(bringup_dir, 'urdf', 'arocs_truck.urdf')
    map_yaml_file = os.path.join(bringup_dir, 'maps', 'infinite_map.yaml')

    # Load URDF file contents into a variable
    with open(truck_urdf_file, 'r') as urdf_file:
        robot_description_content = urdf_file.read()

    # Start Gazebo server with the custom world that includes the truck model
    start_gazebo_server_cmd = ExecuteProcess(
        cmd=['gzserver', '--verbose', world_file, '-s', 'libgazebo_ros_init.so'],
        output='screen')

    # Start Gazebo client
    start_gazebo_client_cmd = ExecuteProcess(
        cmd=['gzclient'],
        output='screen')

    # Start RViz2
    start_rviz_cmd = ExecuteProcess(
        cmd=['rviz2', '-d', rviz_config_file],
        output='screen')

    # Robot state publisher
    robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': True, 'robot_description': robot_description_content}])

    # Map server node
    start_map_server_cmd = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_yaml_file, 'use_sim_time': True}],
    )

    # Lifecycle manager to handle lifecycle state transitions
    lifecycle_manager_cmd = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager',
        output='screen',
        parameters=[{
            'use_sim_time': True,
            'autostart': True,
            'node_names': ['map_server']  # List of lifecycle nodes to manage
        }]
    )

    # Create the launch description and add actions
    ld = LaunchDescription()
    ld.add_action(start_gazebo_server_cmd)
    ld.add_action(start_gazebo_client_cmd)
    ld.add_action(start_rviz_cmd)
    ld.add_action(robot_state_publisher_cmd)
    ld.add_action(start_map_server_cmd)
    ld.add_action(lifecycle_manager_cmd)  # Add the lifecycle manager

    return ld