#!/usr/bin/env python3
"""激光雷达 bringup：启动 YDLiDAR TminiPro 驱动 + 发布 base_footprint→laser_frame 静态外参。

YDLiDAR 驱动节点（ydlidar_ros2_driver_node）内部是普通 rclcpp::Node，启动即在 `scan`
话题持续发布 sensor_msgs/LaserScan，无需 lifecycle configure/activate 转换。

静态外参为**占位值**（绕 z 轴 -90°，对齐 move_try；平移取 0），需在 P3 按 CRAIC 上
激光雷达的实际安装位置标定。可通过 launch 参数 lidar_x/lidar_y/lidar_z/lidar_yaw 覆盖。

用法：
  ros2 launch craic_localization lidar.launch.py
  ros2 launch craic_localization lidar.launch.py params_file:=/abs/path/lidar.yaml lidar_yaw:=-1.5707963
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue


def generate_launch_description():
    pkg_share = get_package_share_directory('craic_localization')
    default_params = os.path.join(pkg_share, 'config', 'lidar.yaml')

    params_file = LaunchConfiguration('params_file')
    lidar_x = LaunchConfiguration('lidar_x')
    lidar_y = LaunchConfiguration('lidar_y')
    lidar_z = LaunchConfiguration('lidar_z')
    lidar_yaw = LaunchConfiguration('lidar_yaw')
    base_frame = LaunchConfiguration('base_frame')
    laser_frame = LaunchConfiguration('laser_frame')
    # 决定数据包解析格式的关键参数，作为 launch 覆盖项暴露，便于排查 Check Sum 报错：
    # 基础 Tmini 无强度 -> intensity:=false；TminiPro 有强度 -> intensity:=true
    intensity = LaunchConfiguration('intensity')
    sample_rate = LaunchConfiguration('sample_rate')
    baudrate = LaunchConfiguration('baudrate')

    return LaunchDescription([
        DeclareLaunchArgument('params_file', default_value=default_params,
                              description='YDLiDAR 参数文件路径'),
        DeclareLaunchArgument('base_frame', default_value='base_footprint'),
        DeclareLaunchArgument('laser_frame', default_value='laser_frame'),
        # 占位外参，P3 标定后覆盖
        DeclareLaunchArgument('lidar_x', default_value='0.0'),
        DeclareLaunchArgument('lidar_y', default_value='0.0'),
        DeclareLaunchArgument('lidar_z', default_value='0.0'),
        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159',
                              description='激光相对底盘偏航角(rad)。-180°=让机体X轴对准底盘实际前进(W键)方向；'
                                          '若实测前后颠倒，改 0.0'),
        # 数据包格式覆盖项（排查 Check Sum 报错用）
        DeclareLaunchArgument('intensity', default_value='true',
                              description='设备是否输出强度：基础 Tmini=false，TminiPro=true。不匹配会刷 Check Sum 报错'),
        DeclareLaunchArgument('sample_rate', default_value='4',
                              description='采样率(K)，Tmini 系列为 4'),
        DeclareLaunchArgument('baudrate', default_value='230400'),

        Node(
            package='ydlidar_ros2_driver',
            executable='ydlidar_ros2_driver_node',
            name='ydlidar_ros2_driver_node',
            output='screen',
            emulate_tty=True,
            parameters=[
                params_file,
                {
                    'intensity': ParameterValue(intensity, value_type=bool),
                    'sample_rate': ParameterValue(sample_rate, value_type=int),
                    'baudrate': ParameterValue(baudrate, value_type=int),
                },
            ],
            namespace='/',
        ),
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='static_tf_base_to_laser',
            arguments=[
                '--x', lidar_x, '--y', lidar_y, '--z', lidar_z,
                '--yaw', lidar_yaw, '--pitch', '0.0', '--roll', '0.0',
                '--frame-id', base_frame, '--child-frame-id', laser_frame,
            ],
        ),
        # base_footprint -> base_link 恒等变换。
        # slam_gmapping(本 build)不从参数读 base_frame，硬编码默认 base_link；
        # 提供该帧让其默认值可解析。本工程 base_footprint 即底盘基准，两者重合。
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='static_tf_base_link',
            arguments=[
                '--x', '0.0', '--y', '0.0', '--z', '0.0',
                '--yaw', '0.0', '--pitch', '0.0', '--roll', '0.0',
                '--frame-id', base_frame, '--child-frame-id', 'base_link',
            ],
        ),
    ])
