#!/usr/bin/env python3
"""定位栈总 bringup：轮式里程计 + 激光雷达 + (可选) rviz。

组成完整 TF 树：odom ──(chassis_odometry)──> base_footprint ──(静态外参)──> laser_frame
用于 P3 激光外参标定与传感器联调：在 rviz 里同时看 /odom 与 /scan，
调 lidar_yaw/lidar_x/lidar_y 直到激光轮廓与实际场地吻合。

用法：
  ros2 launch craic_localization bringup.launch.py
  ros2 launch craic_localization bringup.launch.py use_rviz:=false
  ros2 launch craic_localization bringup.launch.py lidar_yaw:=-1.5707963 intensity:=true
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg_share = get_package_share_directory('craic_localization')
    lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')

    use_rviz = LaunchConfiguration('use_rviz')
    chassis_port = LaunchConfiguration('chassis_port')
    rviz_config = LaunchConfiguration('rviz_config')

    return LaunchDescription([
        DeclareLaunchArgument('use_rviz', default_value='true'),
        DeclareLaunchArgument('chassis_port', default_value='/dev/ttylzucar'),
        DeclareLaunchArgument('rviz_config', default_value=default_rviz),

        # 轮式里程计：odom -> base_footprint
        Node(
            package='craic_localization',
            executable='chassis_odometry',
            name='chassis_odometry',
            output='screen',
            parameters=[{'port': chassis_port}],
        ),

        # 激光雷达 + base_footprint -> laser_frame 静态外参（参数见 lidar.launch.py）
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(lidar_launch),
        ),

        # 可视化（标定 / 联调）
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config],
            condition=IfCondition(use_rviz),
            output='screen',
        ),
    ])
