display.launch.py

import launch
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    bringup_dir = get_package_share_directory('nav2_bringup')
    default_model_path = os.path.join(bringup_dir, 'urdf', 'arocs_truck.urdf')
    default_rviz_config_path = os.path.join(bringup_dir, 'rviz', 'tune_depth_scan.rviz')
    param_file_path = os.path.join(bringup_dir, 'params', 'real_depth_scan_param.yaml')
    pcd_param_file_path = os.path.join(bringup_dir, 'params', 'pointcloud_to_laserscan.yaml')

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[
            {'robot_description': launch_ros.parameter_descriptions.ParameterValue(
                Command(['xacro ', LaunchConfiguration('model')]),
                value_type=str
            )}
        ]
    )

    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
    )
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )
    spawn_entity = launch_ros.actions.Node(
    	package='gazebo_ros',
    	executable='spawn_entity.py',
        arguments=['-entity', 'sam_bot', '-topic', 'robot_description'],
        output='screen'
    )
    robot_localization_node = launch_ros.actions.Node(
         package='robot_localization',
         executable='ekf_node',
         name='ekf_filter_node',
         output='screen',
         parameters=[os.path.join(bringup_dir, 'params/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
    )

    #This TF was already done in xacro file (real_kinect_depth_joint)

    # static_transform_node = launch_ros.actions.Node(
    #     package='tf2_ros',
    #     executable='static_transform_publisher',
    #     arguments=['1', '-1', '0', '0', '0', '0', 'left_lidar_link', 'real_kinect_depth_frame']
    # )



    depth_to_laser_scan_node = launch_ros.actions.Node(
        package='depthimage_to_laserscan',
        executable='depthimage_to_laserscan_node',
        name='depthimage_to_laserscan',
        parameters=[param_file_path],
        remappings=[
            ("depth", "/depth/image_raw"),
            ("depth_camera_info", "/depth/camera_info"),
            ("scan", "/real_kinect_sensor/scan")
        ]
    )

    point_cloud_to_laser_scan_node = launch_ros.actions.Node(
        package='pointcloud_to_laserscan',
        executable='pointcloud_to_laserscan_node',
        name='pointcloud_to_laserscan',
        parameters=[pcd_param_file_path],
        remappings=[
            ("cloud_in", "/points"),
            ("scan", "/real_kinect_sensor/scan")
        ]
    )


    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
                                            description='Flag to enable joint_state_publisher_gui'),
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                            description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                            description='Absolute path to rviz config file'),
        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                            description='Flag to enable use_sim_time'),
        launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'),
        joint_state_publisher_node,
        robot_state_publisher_node,
        spawn_entity,
        robot_localization_node,
        rviz_node,
        #static_transform_node,
        depth_to_laser_scan_node,
        #point_cloud_to_laser_scan_node
    ])