# ============================================================================
# gc_nav2_with_slam_online.launch.py
# 功能：已知全局地图 + SLAM 在线更新 + 导航
#
# 架构：
#                        ┌─ /map（全量静态地图，199×266）
#   map_server ──────────┤
#                        └─ 来自 my_map.yaml + my_map.pgm
#
#                        ┌─ map→odom TF（激光扫描匹配定位）
#   slam_toolbox ────────┤
#   (async mapping)      └─ /slam_map（实时建图结果，重映射避免冲突）
#
#   navigation           ┌─ global_costmap/static_layer ← /map（全量地图）
#                        ├─ local_costmap/obstacle_layer ← /scan（实时避障）
#                        └─ planner + controller + behavior
#
# 关键设计：
#   1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
#   2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
#   3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
#   4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
# ============================================================================

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command
from launch.actions import ExecuteProcess


def generate_launch_description():
    """启动 Gazebo + 已知全局地图 + SLAM 在线定位建图 + Nav2 导航"""
    ld = LaunchDescription()

    # =============================1.包路径================================================================
    pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    origincar_urdf_dir = get_package_share_directory('origincar_description')

    # =============================2.参数==================================================================
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    map_yaml_path = os.path.join(pkg_dir, 'maps', 'zhihui.yaml')
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml')
    posegraph_path = os.path.join(pkg_dir, 'maps', 'zhihui.posegraph')

    # =============================3.map_server：全量静态地图（Foxglove 看到的全局地图）========================
    map_server_node = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': True}],
    )
    map_server_lifecycle = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_map',
        output='screen',
        parameters=[{'use_sim_time': True, 'autostart': True,
                     'node_names': ['map_server']}],
    )

    # =============================4.slam_toolbox：定位 + 实时建图=============================================
    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file, {'use_sim_time': True}],
        remappings=[('/map', '/slam_map')],
    )

    # =============================5.加载已有序列化地图=======================================================
    # 通过 ros2 service call 在 slam_toolbox 启动后加载
    import launch
    load_map_cmd = ExecuteProcess(
        cmd=[
            'bash', '-c',
            # 等待 slam_toolbox 服务就绪后加载地图
            'sleep 8 && '
            'ros2 service call /slam_toolbox/deserialize_map '
            'slam_toolbox/srv/DeserializePoseGraph '
            "'{filename: \"%s\", match_type: 1}'" % posegraph_path,
        ],
        output='screen',
    )

    # =============================6.Nav2 导航栈============================================================
    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': nav2_param_path,
        }.items(),
    )

    # =============================7.Gazebo 仿真============================================================
    robot_name_in_model = 'mycar'
    default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf')
    model = DeclareLaunchArgument(name='model', default_value=default_model_path)
    gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'zhihui.world')
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose',
             '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so',
             gazebo_world_path],
        output='screen',
    )

    # =============================7a.机器人初始位姿参数=====================================================
    # 通过命令行参数覆盖，例如：
    #   ros2 launch ... spawn_x:=1.0 spawn_y:=-2.0 spawn_yaw:=1.57
    spawn_x = LaunchConfiguration('spawn_x', default='-4.311092')
    spawn_y = LaunchConfiguration('spawn_y', default='-4.299756')
    spawn_z = LaunchConfiguration('spawn_z', default='0.0')
    spawn_yaw = LaunchConfiguration('spawn_yaw', default='0.0')

    set_spawn_x = SetEnvironmentVariable('SPAWN_X', spawn_x)
    set_spawn_y = SetEnvironmentVariable('SPAWN_Y', spawn_y)
    set_spawn_z = SetEnvironmentVariable('SPAWN_Z', spawn_z)
    set_spawn_yaw = SetEnvironmentVariable('SPAWN_YAW', spawn_yaw)

    # =============================7b.在 Gazebo 中生成机器人=================================================
    # 通过 bash 读取环境变量，确保位姿参数正确传递
    spawn_entity_cmd = ExecuteProcess(
        cmd=['bash', '-c',
             'ros2 run gazebo_ros spawn_entity.py '
             '-entity mycar '
             f'-file {default_model_path} '
             '-x $SPAWN_X -y $SPAWN_Y -z $SPAWN_Z -Y $SPAWN_YAW'],
        output='screen',
    )

    robot_description = ParameterValue(
        Command(['xacro ', LaunchConfiguration('model')]), value_type=str,
    )
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description,
                     'use_sim_time': True, 'publish_frequency': 30.0}],
    )

    # =============================8.组装==================================================================
    ld.add_action(model)
    ld.add_action(DeclareLaunchArgument(
        'spawn_x', default_value='-4.311092',
        description='机器人初始 x 坐标（m）'))
    ld.add_action(DeclareLaunchArgument(
        'spawn_y', default_value='-4.299756',
        description='机器人初始 y 坐标（m）'))
    ld.add_action(DeclareLaunchArgument(
        'spawn_z', default_value='0.0',
        description='机器人初始 z 坐标（m）'))
    ld.add_action(DeclareLaunchArgument(
        'spawn_yaw', default_value='0.0',
        description='机器人初始偏航角（rad）'))
    ld.add_action(start_gazebo_cmd)
    ld.add_action(set_spawn_x)
    ld.add_action(set_spawn_y)
    ld.add_action(set_spawn_z)
    ld.add_action(set_spawn_yaw)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(robot_state_publisher)
    ld.add_action(map_server_node)
    ld.add_action(map_server_lifecycle)
    ld.add_action(slam_toolbox_node)
    ld.add_action(load_map_cmd)
    ld.add_action(navigation_launch)

    return ld
