#!/usr/bin/env python3
"""P5 精准定位：激光 + rf2o 激光里程计 + 已知地图 + AMCL。

完整 TF 树：map ─(amcl)→ odom ─(rf2o)→ base_footprint ─(静态)→ laser_frame
odom→base_footprint 由 rf2o 从 /scan 连续提供(不依赖底盘里程计板)，AMCL 用它做运动模型、
激光匹配已知地图做校正，输出绝对、无漂移位姿。需要 P4 建好并存盘的地图。

说明：底盘轮式里程计板上电后仅短时上报，暂不参与 odom→base；待其连续上报问题解决后，
可用 robot_localization EKF 融合轮速 + rf2o 再喂 AMCL 提升鲁棒性。

用法：
  ros2 launch craic_localization localization.launch.py map:=/abs/path/maps/craic.yaml
  # 起点不在地图原点时，rviz 用 "2D Pose Estimate" 给初值，或：
  ros2 launch craic_localization localization.launch.py map:=... init_x:=0.3 init_y:=0.2 init_yaw:=1.57
"""

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
from launch_ros.descriptions import ParameterValue


def generate_launch_description():
    pkg_share = get_package_share_directory('craic_localization')
    lidar_launch = os.path.join(pkg_share, 'launch', 'lidar.launch.py')
    amcl_params = os.path.join(pkg_share, 'config', 'amcl.yaml')
    default_rviz = os.path.join(pkg_share, 'rviz', 'localization.rviz')
    default_map = os.path.join(pkg_share, 'maps', 'craic.yaml')

    use_rviz = LaunchConfiguration('use_rviz')
    map_yaml = LaunchConfiguration('map')
    init_x = LaunchConfiguration('init_x')
    init_y = LaunchConfiguration('init_y')
    init_yaw = LaunchConfiguration('init_yaw')
    show_points = LaunchConfiguration('show_points')
    points_file = LaunchConfiguration('points_file')

    return LaunchDescription([
        DeclareLaunchArgument('use_rviz', default_value='true'),
        DeclareLaunchArgument('map', default_value=default_map,
                              description='地图 yaml 路径(P4 由 map_saver 存盘得到)'),
        DeclareLaunchArgument('init_x', default_value='0.0'),
        DeclareLaunchArgument('init_y', default_value='0.0'),
        DeclareLaunchArgument('init_yaw', default_value='0.0'),
        DeclareLaunchArgument('show_points', default_value='true'),
        DeclareLaunchArgument('points_file',
                              default_value=os.path.join(pkg_share, 'config', 'taught_points.yaml'),
                              description='示教点位文件(rviz 显示)；想实时看示教就指向你的 taught_points.yaml'),

        # ===== 传感器 + 激光里程计(odom -> base_footprint) =====
        IncludeLaunchDescription(PythonLaunchDescriptionSource(lidar_launch)),
        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,
            }],
        ),

        # ===== 地图服务 =====
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[amcl_params, {'yaml_filename': map_yaml}],
        ),

        # ===== AMCL 定位 =====
        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[amcl_params, {
                'set_initial_pose': True,
                'initial_pose.x': ParameterValue(init_x, value_type=float),
                'initial_pose.y': ParameterValue(init_y, value_type=float),
                'initial_pose.yaw': ParameterValue(init_yaw, value_type=float),
            }],
        ),

        # ===== 生命周期管理：自动 configure+activate map_server 与 amcl =====
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            parameters=[{
                'use_sim_time': False,
                'autostart': True,
                'node_names': ['map_server', 'amcl'],
            }],
        ),

        # ===== 示教点位可视化 =====
        Node(
            package='craic_localization',
            executable='show_points',
            name='show_points',
            output='screen',
            parameters=[{'points_file': points_file}],
            condition=IfCondition(show_points),
        ),

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