#!/usr/bin/env python3
"""P4 建图：激光 + rf2o 激光里程计 + slam_gmapping(SLAM)。

odom→base_footprint 由 rf2o(从连续 /scan 推算)提供，**不依赖底盘里程计板** ——
该板上电后仅短时上报，会让 odom TF 冻结、gmapping 报 "queue is full" 丢弃所有 scan。
本管线对齐 lzu_robot 可正常建图的做法（rf2o 提供 odom→base，gmapping 出图）。

用法：
  ros2 launch craic_localization mapping.launch.py
  # 另开终端遥控走图；满意后存盘：
  ros2 run nav2_map_server map_saver_cli -f <pkg>/maps/craic --ros-args -p save_map_timeout:=10000.0
"""

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')
    gmapping_params = os.path.join(pkg_share, 'config', 'gmapping.yaml')
    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')

    use_rviz = LaunchConfiguration('use_rviz')
    lidar_x = LaunchConfiguration('lidar_x')
    lidar_y = LaunchConfiguration('lidar_y')
    lidar_z = LaunchConfiguration('lidar_z')
    lidar_yaw = LaunchConfiguration('lidar_yaw')
    intensity = LaunchConfiguration('intensity')
    sample_rate = LaunchConfiguration('sample_rate')
    baudrate = LaunchConfiguration('baudrate')

    return LaunchDescription([
        DeclareLaunchArgument('use_rviz', default_value='true'),
        DeclareLaunchArgument('lidar_x', default_value='0.0',
                              description='base_footprint -> laser_frame 平移 X (m)'),
        DeclareLaunchArgument('lidar_y', default_value='0.0',
                              description='base_footprint -> laser_frame 平移 Y (m)'),
        DeclareLaunchArgument('lidar_z', default_value='0.02',
                              description='base_footprint -> laser_frame 平移 Z (m)'),
        DeclareLaunchArgument('lidar_yaw', default_value='-3.14159',
                              description='base_footprint -> laser_frame 偏航角 (rad)'),
        DeclareLaunchArgument('intensity', default_value='true'),
        DeclareLaunchArgument('sample_rate', default_value='4'),
        DeclareLaunchArgument('baudrate', default_value='230400'),

        # 激光 + 静态外参(base_footprint->laser_frame, base_footprint->base_link)
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(lidar_launch),
            launch_arguments={
                'lidar_x': lidar_x,
                'lidar_y': lidar_y,
                'lidar_z': lidar_z,
                'lidar_yaw': lidar_yaw,
                'intensity': intensity,
                'sample_rate': sample_rate,
                'baudrate': baudrate,
            }.items(),
        ),

        # 激光里程计：从 /scan 连续输出 odom -> base_footprint（建图运动来源）
        Node(
            package='rf2o_laser_odometry',
            executable='rf2o_laser_odometry_node',
            name='rf2o_laser_odometry',
            output='screen',
            parameters=[{
                'laser_scan_topic': '/scan',
                'odom_topic': '/odom',
                'publish_tf': True,
                'base_frame_id': 'base_footprint',
                'odom_frame_id': 'odom',
                'init_pose_from_topic': '',
                'freq': 10.0,
            }],
        ),

        # SLAM：发布 /map 与 TF map -> odom
        Node(
            package='slam_gmapping',
            executable='slam_gmapping',
            name='slam_gmapping',
            output='screen',
            parameters=[gmapping_params],
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', default_rviz],
            condition=IfCondition(use_rviz),
            output='screen',
        ),
    ])