#!/usr/bin/env python3
# ============================================================================
# obstacle_nav2.launch.py
#
# Odometry-only Nav2 navigation using obstacle_scanner detections.
#
# Architecture:
#   origincar_base (bringup)    — chassis serial driver + EKF + TF
#   lslidar_driver              — lidar → /scan
#   obstacle_scanner (optional) — /scan → /obstacles
#   Nav2 navigation_launch      — rolling costmaps + Hybrid A* + MPPI
#
# No map_server, AMCL, or slam_toolbox in the default path.
#
# Usage:
#   ros2 launch obstacle_nav2 obstacle_nav2.launch.py
#   ros2 launch obstacle_nav2 obstacle_nav2.launch.py enable_motion:=true
# ============================================================================

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    GroupAction,
    IncludeLaunchDescription,
    SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, SetRemap
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    """Odometry-only obstacle navigation."""

    pkg_dir = get_package_share_directory('obstacle_nav2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # ========================== Launch Arguments ==============================
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    global_frame = LaunchConfiguration('global_frame', default='odom')
    use_static_map = LaunchConfiguration('use_static_map', default='false')
    map_yaml = LaunchConfiguration('map_yaml', default='')
    enable_motion = LaunchConfiguration('enable_motion', default='true')
    start_base = LaunchConfiguration('start_base', default='true')
    start_lidar = LaunchConfiguration('start_lidar', default='true')
    start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
    start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
    map_yaml_path = LaunchConfiguration('map_yaml_path')

    nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
    nav_to_pose_bt_path = os.path.join(
        pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
    nav_through_poses_bt_path = os.path.join(
        pkg_dir, 'behavior_tree', 'nav_through_poses_ackermann.xml')
    configured_params = RewrittenYaml(
        source_file=nav2_param_path,
        root_key='',
        param_rewrites={
            'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
            'default_nav_through_poses_bt_xml': nav_through_poses_bt_path,
        },
        convert_types=True,
    )
    base_cmd_vel_topic = PythonExpression([
        "'/cmd_vel' if '", enable_motion,
        "' == 'true' else '/cmd_vel_hardware_disabled'",
    ])

    # ========================== Base Bringup ==================================
    origincar_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [get_package_share_directory('origincar_base'),
             '/launch', '/origincar_bringup.launch.py']),
        condition=IfCondition(start_base),
    )
    safe_base_bringup = GroupAction([
        SetRemap(src='cmd_vel', dst=base_cmd_vel_topic),
        origincar_bringup,
    ])

    # ========================== Lidar Driver ==================================
    lslidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [get_package_share_directory('lslidar_driver'),
             '/launch', '/lsn10_launch.py']),
        condition=IfCondition(start_lidar),
    )

    # ========================== Obstacle Scanner ==============================
    obstacle_scanner_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [get_package_share_directory('obstacle_scanner'),
             '/launch', '/obstacle_scanner.launch.py']),
        condition=IfCondition(start_obstacle_scanner),
    )

    # ========================== Static Map Publisher ==========================
    static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
    static_map_publisher = ExecuteProcess(
        cmd=[
            FindExecutable(name='python3'),
            static_map_publisher_script,
            '--ros-args',
            '-p', ['yaml_filename:=', map_yaml_path],
            '-p', 'publish_rate:=1.0',
            '-p', 'map_frame:=map',
        ],
        output='screen',
        emulate_tty=True,
        condition=IfCondition(start_map_publisher),
    )

    # ========================== Initial Pose -> TF (map->odom) ================
    initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
    initial_pose_to_tf = ExecuteProcess(
        cmd=[
            FindExecutable(name='python3'),
            initial_pose_to_tf_script,
            '--ros-args',
            '-p', 'odom_frame:=odom',
            '-p', 'base_frame:=base_footprint',
            '-p', 'map_frame:=map',
        ],
        output='screen',
        emulate_tty=True,
    )

    # ========================== Nav2 Navigation ===============================
    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': configured_params,
            'autostart': 'true',
        }.items(),
    )

    # ========================== Assembly ======================================
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock'),
        DeclareLaunchArgument(
            'global_frame',
            default_value='odom',
            description='Global frame for both costmaps'),
        DeclareLaunchArgument(
            'use_static_map',
            default_value='false',
            description='Reserved: enable static map + AMCL (not yet implemented)'),
        DeclareLaunchArgument(
            'map_yaml',
            default_value='',
            description='Reserved: path to map YAML file'),
        DeclareLaunchArgument(
            'enable_motion',
            default_value='true',
            description='Route Nav2 cmd_vel to the real base topic'),
        DeclareLaunchArgument(
            'start_base',
            default_value='true',
            description='Start the base driver, EKF, and robot TF publishers'),
        DeclareLaunchArgument(
            'start_lidar',
            default_value='true',
            description='Start the lidar driver'),
        DeclareLaunchArgument(
            'start_obstacle_scanner',
            default_value='true',
            description='Also start the obstacle_scanner node'),
        DeclareLaunchArgument(
            'start_map_publisher',
            default_value='true',
            description='Start the static map publisher node'),
        DeclareLaunchArgument(
            'map_yaml_path',
            default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
            description='Path to the map YAML file'),


        safe_base_bringup,
        lslidar_launch,
        obstacle_scanner_launch,
        static_map_publisher,
        initial_pose_to_tf,
        navigation_launch,
    ])
