#!/usr/bin/env python3
"""
obstacle_nav2_q_waypoints.launch.py
====================================
复合启动文件：同时启动 obstacle_nav2 导航系统 + Q 路点候选选择器节点。

架构概览
--------
本 launch 文件包含两个核心组件：
  1. obstacle_nav2  : 完整的障碍物导航栈（底盘驱动、激光雷达、Nav2 导航、障碍物扫描）
  2. q_waypoint_selector : 独立的 Python 节点，周期性从 costmap 中评估并选择
     最优的 Q 路点候选目标，通过 NavigateToPose action 下发导航目标。

Q 路点选择器的设计意图
----------------------
在比赛中，我们需要依次访问一系列路点（Q₁, Q₂, ..., Qₙ），但每个路点可能有多个
候选位置。选择器从 JSON 文件中加载有序路点及其候选坐标，结合全局 costmap 的
占用栅格信息，过滤掉被障碍物占据的候选点，并向 Nav2 下发当前最优的导航目标。

启动方式
--------
  ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
      use_sim_time:=false \
      global_frame:=odom \
      json_dir:=/path/to/json \
      goal_frame:=odom \
      costmap_topic:=/global_costmap/costmap
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.substitutions import FindExecutable


def generate_launch_description():
    # =========================================================================
    # 路径配置 —— 定位 obstacle_nav2 包内的关键文件
    # =========================================================================

    # obstacle_nav2 包的共享目录（install 后的 share/obstacle_nav2）
    pkg_dir = get_package_share_directory("obstacle_nav2")

    # 基础导航 launch 文件路径，将被 IncludeLaunchDescription 引入
    base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")

    # Q 路点选择器的 Python 脚本路径，将被 ExecuteProcess 作为独立进程运行
    selector_script = os.path.join(pkg_dir, "config", "q_waypoint_selector.py")

    # 默认的 JSON 路点文件目录
    default_json_dir = os.path.join(pkg_dir, "config", "json")

    # =========================================================================
    # LaunchConfiguration —— 运行时参数占位符
    # 这些变量在 launch 时被替换为实际的参数值（命令行传入或默认值）。
    # 使用 LaunchConfiguration 而非直接读取值，是为了支持 launch 系统的
    # 延迟求值（lazy evaluation）机制。
    # =========================================================================

    # ---- obstacle_nav2 基础导航栈参数 ----

    # 是否使用仿真时间（实车=false，Gazebo 仿真=true）
    use_sim_time = LaunchConfiguration("use_sim_time")

    # 全局坐标系名称（仿真用 "odom"，实车用 "odom_combined"）
    global_frame = LaunchConfiguration("global_frame")

    # 是否加载静态地图（已知地图导航=true，纯 SLAM 探索=false）
    use_static_map = LaunchConfiguration("use_static_map")

    # 静态地图 YAML 文件路径（为空时不加载 map_server）
    map_yaml = LaunchConfiguration("map_yaml")

    # 是否启用底盘运动控制（调试时可关闭以只运行感知）
    enable_motion = LaunchConfiguration("enable_motion")

    # 是否启动底盘驱动节点（实车=true，纯仿真可视化=false）
    start_base = LaunchConfiguration("start_base")

    # 是否启动激光雷达驱动节点
    start_lidar = LaunchConfiguration("start_lidar")

    # 是否启动障碍物扫描节点（obstacle_scanner）
    start_obstacle_scanner = LaunchConfiguration("start_obstacle_scanner")

    # ---- Q 路点选择器专属参数 ----

    # JSON 路点文件所在目录，包含如 Q1.json, Q2.json 等有序路点定义
    json_dir = LaunchConfiguration("json_dir")

    # 下发 NavigateToPose 导航目标时使用的坐标系
    goal_frame = LaunchConfiguration("goal_frame")

    # 全局 costmap 话题，用于判断候选路点是否被障碍物占据
    costmap_topic = LaunchConfiguration("costmap_topic")

    # Nav2 的 NavigateToPose action 名称
    navigate_action = LaunchConfiguration("navigate_action")

    # 占用阈值：costmap 栅格值 >= 此值的单元格视为被占用（范围 0-100）
    occupied_threshold = LaunchConfiguration("occupied_threshold")

    # 是否将未知区域（costmap 值 == -1）视为占用
    treat_unknown_as_occupied = LaunchConfiguration("treat_unknown_as_occupied")

    # 单个候选目标的超时时间（秒），超时后取消当前目标并尝试下一个候选
    goal_timeout_sec = LaunchConfiguration("goal_timeout_sec")

    # 选择器的控制周期（秒），即多久重新评估一次最优候选路点
    selection_period_sec = LaunchConfiguration("selection_period_sec")

    # =========================================================================
    # 组件 1 —— obstacle_nav2 基础导航栈
    # 通过 IncludeLaunchDescription 引入 obstacle_nav2.launch.py，
    # 将其作为一个子 launch 嵌入当前 launch 描述中。
    # 传入的参数会透传给子 launch 文件，实现参数的统一管理。
    # =========================================================================
    obstacle_nav2 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(base_launch),
        launch_arguments={
            "use_sim_time": use_sim_time,
            "global_frame": global_frame,
            "use_static_map": use_static_map,
            "map_yaml": map_yaml,
            "enable_motion": enable_motion,
            "start_base": start_base,
            "start_lidar": start_lidar,
            "start_obstacle_scanner": start_obstacle_scanner,
        }.items(),
    )

    # =========================================================================
    # 组件 2 —— Q 路点候选选择器
    # 作为一个独立的 Python 进程运行（ExecuteProcess），而非 ROS 2 Node action。
    # 选择器内部会创建一个 rclpy 节点，订阅 costmap、发布导航目标。
    # 参数通过 ROS 2 标准的 --ros-args -p 方式传递。
    # =========================================================================
    q_waypoint_selector = ExecuteProcess(
        cmd=[
            # 使用 FindExecutable 查找 python3 可执行文件，确保跨平台兼容
            FindExecutable(name="python3"),
            selector_script,
            # ROS 2 命令行参数传递标准格式
            "--ros-args",
            "-p", ["json_dir:=", json_dir],
            "-p", ["goal_frame:=", goal_frame],
            "-p", ["costmap_topic:=", costmap_topic],
            "-p", ["navigate_action:=", navigate_action],
            "-p", ["occupied_threshold:=", occupied_threshold],
            "-p", ["treat_unknown_as_occupied:=", treat_unknown_as_occupied],
            "-p", ["goal_timeout_sec:=", goal_timeout_sec],
            "-p", ["selection_period_sec:=", selection_period_sec],
            "-p", ["use_sim_time:=", use_sim_time],
        ],
        # 将进程的标准输出打印到终端，便于调试
        output="screen",
        emulate_tty=True,
    )

    # =========================================================================
    # LaunchDescription —— 组装并返回完整的 launch 描述
    # 包含三部分：
    #   1. DeclareLaunchArgument —— 声明所有可配置参数及其默认值
    #   2. obstacle_nav2           —— 基础导航栈
    #   3. q_waypoint_selector     —— Q 路点选择器
    # =========================================================================
    return LaunchDescription(
        [
            # =================================================================
            # obstacle_nav2 基础导航栈参数声明
            # =================================================================

            # 是否使用仿真时间（/clock 话题），实车场景应设为 false
            DeclareLaunchArgument("use_sim_time", default_value="false"),

            # 全局坐标系：仿真用 "odom"，实车 EKF 融合后用 "odom_combined"
            DeclareLaunchArgument("global_frame", default_value="odom"),

            # 是否启用静态地图层（map_server），在线 SLAM 建图时设为 false
            DeclareLaunchArgument("use_static_map", default_value="false"),

            # 静态地图 yaml 文件路径，为空字符串时不加载地图
            DeclareLaunchArgument("map_yaml", default_value=""),

            # 是否启用底盘运动控制指令下发，调试时可关闭
            DeclareLaunchArgument("enable_motion", default_value="true"),

            # 是否启动底盘串口驱动节点（实车需要，仿真不需要）
            DeclareLaunchArgument("start_base", default_value="true"),

            # 是否启动激光雷达驱动节点
            DeclareLaunchArgument("start_lidar", default_value="true"),

            # 是否启动障碍物扫描与避障节点
            DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),

            # =================================================================
            # Q 路点选择器参数声明
            # =================================================================

            # JSON 路点文件目录，每个 JSON 文件定义一个路点的候选坐标列表
            DeclareLaunchArgument(
                "json_dir",
                default_value=default_json_dir,
                description="Directory containing ordered waypoint JSON files.",
            ),

            # 导航目标使用的坐标帧，应与全局 costmap 的坐标系一致
            DeclareLaunchArgument(
                "goal_frame",
                default_value="odom",
                description="Frame used for NavigateToPose goals.",
            ),

            # 全局 costmap 话题，选择器订阅此话题来评估候选点的可通行性
            DeclareLaunchArgument(
                "costmap_topic",
                default_value="/global_costmap/costmap",
                description="OccupancyGrid topic used to reject occupied candidates.",
            ),

            # NavigateToPose action 的服务名，与 Nav2 的 bt_navigator 对应
            DeclareLaunchArgument(
                "navigate_action",
                default_value="/navigate_to_pose",
                description="Nav2 NavigateToPose action name.",
            ),

            # costmap 占用判定阈值（0-100），>= 此值的栅格视为障碍物
            DeclareLaunchArgument(
                "occupied_threshold",
                default_value="50",
                description="Costmap cells >= this value are considered occupied.",
            ),

            # 是否将未知区域（costmap 值 == -1）视为不可通行
            DeclareLaunchArgument(
                "treat_unknown_as_occupied",
                default_value="true",
                description="Treat costmap value -1 as occupied.",
            ),

            # 单个导航目标的超时时间，超时后自动取消并切换候选
            DeclareLaunchArgument(
                "goal_timeout_sec",
                default_value="60.0",
                description="Cancel a candidate goal if it does not finish before this timeout.",
            ),

            # 选择器主循环的控制周期（秒），值越小响应越快但 CPU 开销越大
            DeclareLaunchArgument(
                "selection_period_sec",
                default_value="0.5",
                description="Selector control period.",
            ),

            # ---- 组装已配置的 action ----
            obstacle_nav2,
            q_waypoint_selector,
        ]
    )
