#!/usr/bin/python3
# -- coding: utf-8 --**
# cam2 전용 launch — mid360s.yaml + extrin_cam2.yaml + camera_cam2.yaml (4mm, 2026-07-08 캘리브레이션)

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():

    pkg_dir = get_package_share_directory("fast_livo")
    config_file_dir = os.path.join(pkg_dir, "config")
    rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
    urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")

    mid360s_config_cmd  = os.path.join(config_file_dir, "mid360s.yaml")
    extrin_config_cmd   = os.path.join(config_file_dir, "extrin_cam2.yaml")
    camera_config_cmd   = os.path.join(config_file_dir, "camera_cam2.yaml")

    use_rviz_arg = DeclareLaunchArgument(
        "use_rviz", default_value="False", description="Whether to launch Rviz2"
    )
    mid360s_config_arg = DeclareLaunchArgument(
        "mid360s_params_file", default_value=mid360s_config_cmd,
        description="Full path to mid360s.yaml"
    )
    extrin_config_arg = DeclareLaunchArgument(
        "extrin_params_file", default_value=extrin_config_cmd,
        description="Full path to cam2 extrinsic override yaml"
    )
    camera_config_arg = DeclareLaunchArgument(
        "camera_params_file", default_value=camera_config_cmd,
        description="Full path to camera intrinsics yaml (cam2, 4mm)"
    )
    use_respawn_arg = DeclareLaunchArgument(
        "use_respawn", default_value="True", description="Whether to respawn if a node crashes."
    )

    mid360s_params_file = LaunchConfiguration("mid360s_params_file")
    extrin_params_file  = LaunchConfiguration("extrin_params_file")
    camera_params_file  = LaunchConfiguration("camera_params_file")
    use_respawn         = LaunchConfiguration("use_respawn")

    return LaunchDescription([
        use_rviz_arg,
        mid360s_config_arg,
        extrin_config_arg,
        camera_config_arg,
        use_respawn_arg,

        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            name="robot_state_publisher",
            parameters=[{"robot_description": open(urdf_file).read()}],
            output="screen"
        ),

        Node(
            package="demo_nodes_cpp",
            executable="parameter_blackboard",
            name="parameter_blackboard",
            parameters=[camera_params_file],
            output="screen"
        ),

        Node(
            package="fast_livo",
            executable="fastlivo_mapping",
            name="laserMapping",
            parameters=[
                mid360s_params_file,
                extrin_params_file,
                {"common": {"img_topic": "/cam2/image"}},
            ],
            additional_env={
                "LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
            },
            output="screen"
        ),

        Node(
            condition=IfCondition(LaunchConfiguration("use_rviz")),
            package="rviz2",
            executable="rviz2",
            name="rviz2",
            arguments=["-d", rviz_config_file],
            output="screen"
        ),
    ])
