#!/usr/bin/env python3
# ============================================================================
# gc_slam_mapping.launch.py
# 功能：Gazebo 仿真环境 + SLAM 建图启动文件
#
# 适用场景：
#   - 在 Gazebo 仿真中遥控机器人建立环境地图
#   - 在已有地图基础上增量更新
#   - 单独调试 slam_toolbox 建图参数
#
# 架构：
#   Gazebo 仿真器          — 加载 zhihui.world（智慧楼墙体环境）
#   spawn_entity           — 在 Gazebo 中生成机器人模型
#   robot_state_publisher  — 发布机器人 TF 树（URDF 模型）
#   joint_state_publisher  — 发布关节状态
#   async_slam_toolbox_node — 激光 SLAM 建图（mapping 模式）
#                             发布 map→odom TF
#                             发布 /map 话题（实时地图）
#
# 使用方式：
#   ros2 launch gc_navigation2_slamtoolbox gc_slam_mapping.launch.py
#
# 保存地图：
#   ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
#     "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map'}"
#   ros2 run nav2_map_server map_saver_cli -f <map_name>
# ============================================================================

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


def generate_launch_description():
    """生成 LaunchDescription，启动 Gazebo + slam_toolbox 建图"""

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

    # ============================ 2. 声明启动参数 ====================================
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    # slam_params_file: slam_toolbox 配置文件
    slam_params_file = LaunchConfiguration(
        'slam_params_file',
        default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'))

    # map_file_name: 可选，加载已有序列化地图继续建图
    map_file_name = LaunchConfiguration('map_file_name', default='')

    # model: 机器人 URDF 模型路径（用于 xacro 处理）
    # origincar.urdf 是纯 URDF（不含 xacro 宏），xacro 会原样透传
    default_model_path = os.path.join(
        origincar_urdf_dir, 'urdf', 'origincar.urdf')
    model = DeclareLaunchArgument(
        name='model', default_value=default_model_path)

    # spawn_model: Gazebo 中生成机器人用的 URDF（不含 xacro 宏）
    spawn_model_path = os.path.join(
        origincar_urdf_dir, 'urdf', 'origincar.urdf')

    # ============================ 3. 设置 GAZEBO_MODEL_PATH ==========================
    # 让 Gazebo 能找到 zhihui 模型，同时保留标准模型路径
    gazebo_model_path = os.path.join(pkg_dir, 'world')
    existing_gazebo_path = os.environ.get('GAZEBO_MODEL_PATH', '')
    if existing_gazebo_path:
        full_gazebo_path = f'{gazebo_model_path}:{existing_gazebo_path}'
    else:
        full_gazebo_path = gazebo_model_path
    set_gazebo_model_path = SetEnvironmentVariable(
        name='GAZEBO_MODEL_PATH',
        value=full_gazebo_path
    )

    # ============================ 4. 启动 Gazebo 仿真器 ==============================
    gazebo_world_path = os.path.join(pkg_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'
    )

    # ============================ 5. 机器人初始位姿参数 ==============================
    # 通过命令行参数覆盖，例如：
    #   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')

    # 将位姿参数设为环境变量，供 spawn 命令使用
    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)

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

    # ============================ 7. robot_state_publisher ===========================
    robot_description = ParameterValue(
        Command(['xacro ', LaunchConfiguration('model')]),
        value_type=str
    )
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': robot_description,
            'use_sim_time': True,
            'publish_frequency': 30.0,
        }],
    )

    # ============================ 8. joint_state_publisher ===========================
    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
    )

    # ============================ 9. slam_toolbox 建图节点 ============================
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[
            slam_params_file,
            {
                'use_sim_time': use_sim_time,
                'map_file_name': map_file_name,
            },
        ],
    )

    # ============================ 10. 组装 LaunchDescription =========================
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='使用仿真时间（Gazebo=true, 实车=false)'),
        DeclareLaunchArgument(
            'slam_params_file',
            default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async.yaml'),
            description='slam_toolbox 参数配置文件路径'),
        DeclareLaunchArgument(
            'map_file_name',
            default_value='',
            description='已有序列化地图路径（.posegraph），留空则从零开始建图'),
        DeclareLaunchArgument(
            'spawn_x', default_value='-4.311092',
            description='机器人初始 x 坐标（m）'),
        DeclareLaunchArgument(
            'spawn_y', default_value='-4.299756',
            description='机器人初始 y 坐标（m）'),
        DeclareLaunchArgument(
            'spawn_z', default_value='0.0',
            description='机器人初始 z 坐标（m）'),
        DeclareLaunchArgument(
            'spawn_yaw', default_value='0.0',
            description='机器人初始偏航角（rad）'),
        model,

        set_gazebo_model_path,
        set_spawn_x,
        set_spawn_y,
        set_spawn_z,
        set_spawn_yaw,
        start_gazebo_cmd,
        spawn_entity_cmd,
        robot_state_publisher_node,
        joint_state_publisher_node,
        slam_toolbox_node,
    ])