#!/usr/bin/env python3

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node

def generate_launch_description():
    rs_launch_args = {
        'depth_module.depth_profile': '848x480x30',
        'rgb_camera.color_profile': '848x480x30',
        'pointcloud.enable': 'true',
        'enable_rgbd': 'true',
        'enable_sync': 'true',
        'align_depth.enable': 'true',
        'enable_color': 'true',
        'enable_depth': 'true',
        'publish_tf': 'true',
        'tf_publish_rate': '10.0',
        'accelerate_gpu_with_glsl': 'true',
        'unite_imu_method': '0',
        'depth_module.emitter_enabled': '1',
        'serial_no': '_250122079842',	# D435
        'camera_name': 'D435',
        'camera_namespace': 'camera',
    }

    rs_multi_camera_launch_args = {
        'pointcloud.enable1': 'true',
        'pointcloud.enable2': 'true',
        'enable_rgbd1': 'true',
        'enable_rgbd2': 'true',
        'enable_sync1': 'true',
        'enable_sync2': 'true',
        'align_depth.enable1': 'true',
        'align_depth.enable2': 'true',
        'enable_color1': 'true',
        'enable_color2': 'true',
        'enable_depth1': 'true',
        'enable_depth2': 'true',
        'publish_tf1': 'true',
        'publish_tf2': 'true',
        'tf_publish_rate1': '10.0',
        'tf_publish_rate2': '10.0',
        'unite_imu_method1': '0',
        'unite_imu_method2': '0',
        'accelerate_gpu_with_glsl1': 'true',
        'accelerate_gpu_with_glsl2': 'true',
        'serial_no1': '_218622279009',	# D405
        'serial_no2': '_218622277391',	# D405
        'camera_name1': 'D405_1',
        'camera_namespace1': 'camera',
        'camera_name2': 'D405_2',
        'camera_namespace2': 'camera',
    }
    
    rs_launch = IncludeLaunchDescription(
    	PythonLaunchDescriptionSource(
    		PathJoinSubstitution([FindPackageShare('realsense2_camera'), 'launch', 'rs_launch.py'])
    	),
    	launch_arguments=rs_launch_args.items()
    )
    
    rs_multi_camera_launch = IncludeLaunchDescription(
    	PythonLaunchDescriptionSource(
    		PathJoinSubstitution([FindPackageShare('realsense2_camera'), 'launch', 'rs_multi_camera_launch.py'])
    	),
    	launch_arguments=rs_multi_camera_launch_args.items()
    )
    
    # TF для D435 (спереди, смотрит вперёд)
    D435_static_tf = Node(
    	package="tf2_ros",
    	executable="static_transform_publisher",
    	arguments=[
    		'--x', '0', '--y', '0', '--z', '0', '--yaw', '0', '--pitch', '0', '--roll', '0',
    		'--frame-id', 'base_link', '--child-frame-id', 'D435_link'
    	]
    )
    
    # TF для D405 #1 (слева, смотрит влево)
    D405_1_static_tf = Node(
    	package="tf2_ros",
    	executable="static_transform_publisher",
    	arguments=[
    		'--x', '-0.031', '--y', '0.21', '--z', '0.003', '--yaw', '0', '--pitch', '0', '--roll', '0',
    		'--frame-id', 'base_link', '--child-frame-id', 'D405_1_link'
    	]
    )
    
    # TF для D405 #2 (справа, смотрит вправо)
    D405_2_static_tf = Node(
    	package="tf2_ros",
    	executable="static_transform_publisher",
    	arguments=[
    		'--x', '-0.015', '--y', '-0.235', '--z', '0.003', '--yaw', '0', '--pitch', '0', '--roll', '0',
    		'--frame-id', 'base_link', '--child-frame-id', 'D405_2_link'
    	]
    )
    
    multi_camera_rtabmap = IncludeLaunchDescription(
    	PythonLaunchDescriptionSource(
    		PathJoinSubstitution([FindPackageShare('rtabmap_examples'), 'launch', 'rtabmap_D405x3.launch.py'])
    	)
    )

    return LaunchDescription([
    	D435_static_tf,      # Сначала TF
        D405_1_static_tf,
        D405_2_static_tf,
        rs_launch,             # Потом камеры
        rs_multi_camera_launch,
        multi_camera_rtabmap   # И SLAM
    ])