#!/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')

    return LaunchDescription([
        DeclareLaunchArgument('use_rviz', default_value='true'),

        # 激光 + 静态外参(base_footprint->laser_frame, base_footprint->base_link)
        IncludeLaunchDescription(PythonLaunchDescriptionSource(lidar_launch)),

        # 激光里程计：从 /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',
        ),
    ])
