#!/usr/bin/env python3
# Copyright 2019 ROBOTIS CO., LTD.
# SPDX-License-Identifier: Apache-2.0
#
# Gazebo Sim 8 (Harmonic) + ROS 2 Humble — TurtleBot3 办公室 Task1 链路：
# 默认加载 office_gz_dartsim.sdf（已去除网格碰撞，dartsim 稳定）+ GPU 激光 + Nav2/SLAM 所需话题。

import os
from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    LogInfo,
    OpaqueFunction,
    SetEnvironmentVariable,
    TimerAction,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node


def _turtlebot3_gazebo_share():
    try:
        return get_package_share_directory('turtlebot3_gazebo')
    except LookupError:
        return str(Path(__file__).resolve().parent.parent)


def _launch_setup(context, *args, **kwargs):
    pkg_share = _turtlebot3_gazebo_share()
    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')

    world = context.launch_configurations['world']
    gz_partition = context.launch_configurations['gz_partition']
    world_name = context.launch_configurations['gz_world_name']
    physics = context.launch_configurations['physics_engine']
    headless = context.launch_configurations['headless'].lower() == 'true'
    x_pose = context.launch_configurations['x_pose']
    y_pose = context.launch_configurations['y_pose']
    use_sim_time = context.launch_configurations['use_sim_time']
    robot_model = context.launch_configurations['robot_model']

    model_gz = os.path.abspath(
        os.path.join(pkg_share, 'models', f'turtlebot3_{robot_model}', 'model_gz.sdf'))
    if not os.path.isfile(model_gz):
        raise RuntimeError(
            f'Missing Gazebo Sim model: {model_gz}. Supported robot_model: burger, waffle.'
        )

    world_abs = os.path.abspath(os.path.expanduser(world))
    if not os.path.isfile(world_abs):
        raise RuntimeError(
            f'World file not found: {world_abs}. '
            'Build/install turtlebot3_gazebo or pass world:=/absolute/path/office_gz_dartsim.sdf'
        )

    gz_entity = f'turtlebot3_{robot_model}'
    gz_prefix = '-s ' if headless else ''
    gz_args = f'{gz_prefix}-r --physics-engine {physics} {world_abs}'

    gz_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
        launch_arguments={'gz_args': gz_args}.items(),
    )

    urdf_name = f'turtlebot3_{robot_model}.urdf'
    urdf_path = os.path.join(pkg_share, 'urdf', urdf_name)
    with open(urdf_path, 'r', encoding='utf-8') as urdf_file:
        robot_desc = urdf_file.read()

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'use_sim_time': use_sim_time.lower() == 'true',
            'robot_description': robot_desc,
        }],
    )

    joint_gz = f'/world/{world_name}/model/{gz_entity}/joint_state'
    cmd_topic = f'/model/{gz_entity}/cmd_vel'
    odom_topic = f'/model/{gz_entity}/odometry'
    # DiffDrive publishes odom->base_footprint as gz.msgs.Pose_V on this topic.
    tf_topic = f'/model/{gz_entity}/tf'

    # Gazebo sometimes publishes LaserScan frame_id as "<model>/base_scan/hls_lfcd_lds" even with
    # gz_frame_id; SLAM/RViz expect "base_scan" (URDF). Identity tie fixes TF without changing hits.
    gz_lidar_frame = f'{gz_entity}/base_scan/hls_lfcd_lds'
    lidar_frame_broadcaster = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='gz_lidar_frame_align',
        arguments=[
            '--frame-id', 'base_scan',
            '--child-frame-id', gz_lidar_frame,
        ],
        parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
    )

    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
            f'{cmd_topic}@geometry_msgs/msg/Twist@gz.msgs.Twist',
            f'{odom_topic}@nav_msgs/msg/Odometry@gz.msgs.Odometry',
            f'{joint_gz}@sensor_msgs/msg/JointState[gz.msgs.Model',
            f'{tf_topic}@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
            '/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan',
            '/camera@sensor_msgs/msg/Image@gz.msgs.Image',
            '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
        ],
        remappings=[
            (joint_gz, '/joint_states'),
            (tf_topic, '/tf'),
            (cmd_topic, '/cmd_vel'),
            (odom_topic, '/odom'),
            ('/camera', '/camera/image_raw'),
            ('/camera_info', '/camera/camera_info'),
        ],
        parameters=[{
            f'qos_overrides.{cmd_topic}.subscriber.reliability': 'reliable',
        }],
        output='screen',
    )

    spawn = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-world', world_name,
            '-file', model_gz,
            '-name', gz_entity,
            '-x', x_pose,
            '-y', y_pose,
            '-z', '0.05',
        ],
        output='screen',
    )

    delayed_spawn = TimerAction(period=12.0, actions=[spawn])

    set_gz_partition = SetEnvironmentVariable(name='GZ_PARTITION', value=gz_partition)

    set_gz_res = SetEnvironmentVariable(
        name='GZ_SIM_RESOURCE_PATH',
        value=os.pathsep.join([
            os.path.join(pkg_share, 'models', 'turtlebot3_office'),
            os.path.join(pkg_share, 'models'),
            os.path.join(Path.home(), '.gazebo', 'models'),
        ]),
    )

    log_start = LogInfo(msg=[
        '[turtlebot3_office_gz] Isolated GZ_PARTITION='
        + gz_partition
        + ' (close other gz sim windows or they stay on the default partition). World='
        + world_abs,
    ])

    actions = [
        set_gz_partition,
        set_gz_res,
        log_start,
        robot_state_publisher,
        lidar_frame_broadcaster,
        gz_sim,
        bridge,
        delayed_spawn,
    ]

    start_slam = context.launch_configurations['start_slam'].lower() == 'true'
    if start_slam:
        try:
            slam_pkg = get_package_share_directory('slam_toolbox')
        except LookupError:
            slam_pkg = ''
        if slam_pkg:
            slam_launch = os.path.join(slam_pkg, 'launch', 'online_async_launch.py')
            slam_params = context.launch_configurations['slam_params_file']
            if not os.path.isfile(slam_params):
                raise RuntimeError(
                    f'slam_params_file not found: {slam_params} '
                    '(install turtlebot3_gazebo + config, or pass slam_params_file:=...)'
                )
            actions.append(
                TimerAction(
                    period=14.0,
                    actions=[
                        LogInfo(msg=[
                            '[turtlebot3_office_gz] Starting slam_toolbox (map frame will appear for RViz).',
                        ]),
                        IncludeLaunchDescription(
                            PythonLaunchDescriptionSource(slam_launch),
                            launch_arguments={
                                'use_sim_time': context.launch_configurations['use_sim_time'],
                                'slam_params_file': slam_params,
                            }.items(),
                        ),
                    ],
                ))
        else:
            actions.append(
                LogInfo(msg=[
                    '[turtlebot3_office_gz] start_slam=true but slam_toolbox package not found '
                    '(install ros-humble-slam-toolbox).',
                ]))

    if context.launch_configurations['show_rviz'].lower() == 'true':
        try:
            nav2_share = get_package_share_directory('nav2_bringup')
            rviz_cfg = os.path.join(nav2_share, 'rviz', 'nav2_default_view.rviz')
        except LookupError:
            rviz_cfg = ''
        if rviz_cfg and os.path.isfile(rviz_cfg):
            rviz_node = Node(
                package='rviz2',
                executable='rviz2',
                name='rviz2',
                arguments=['-d', rviz_cfg],
                parameters=[{'use_sim_time': use_sim_time.lower() == 'true'}],
                output='screen',
            )
            # RViz fixed frame is "map": wait for slam_toolbox; otherwise scans pile up and drop.
            if start_slam:
                actions.append(
                    TimerAction(
                        period=20.0,
                        actions=[
                            LogInfo(msg=[
                                '[turtlebot3_office_gz] Starting RViz (after SLAM). Drive the robot to map the office.',
                            ]),
                            rviz_node,
                        ],
                    ))
            else:
                actions.append(rviz_node)
        else:
            actions.append(
                LogInfo(msg=[
                    '[turtlebot3_office_gz] show_rviz=true but nav2_bringup not found or '
                    'missing nav2_default_view.rviz — install ros-humble-nav2-bringup or '
                    'run rviz2 manually.',
                ]))

    return actions


def generate_launch_description():
    pkg_share = _turtlebot3_gazebo_share()
    default_world = os.path.join(pkg_share, 'worlds', 'office_gz_dartsim.sdf')
    default_slam_params = os.path.join(pkg_share, 'config', 'mapper_params_office_gz_sim.yaml')
    default_robot = os.environ.get('TURTLEBOT3_MODEL', 'burger')

    return LaunchDescription([
        DeclareLaunchArgument(
            'world',
            default_value=default_world,
            description='SDF world file (must match gz_world_name in the file).',
        ),
        DeclareLaunchArgument(
            'gz_partition',
            default_value='tb3_office_gz',
            description='Unique Gazebo transport partition so this sim does not attach to another gz sim '
            '(e.g. tutorial playground / gpu_lidar) on the same machine.',
        ),
        DeclareLaunchArgument(
            'gz_world_name',
            default_value='default',
            description='<world name="..."> (office_gz_dartsim.sdf → default).',
        ),
        DeclareLaunchArgument(
            'physics_engine',
            default_value='gz-physics-dartsim-plugin',
            description='Physics engine plugin for gz-sim.',
        ),
        DeclareLaunchArgument(
            'headless',
            default_value='false',
            description='If true, run gz sim server only (-s).',
        ),
        DeclareLaunchArgument(
            'robot_model',
            default_value=default_robot,
            description='TurtleBot3 variant: burger or waffle (must match TURTLEBOT3_MODEL).',
        ),
        DeclareLaunchArgument('x_pose', default_value='0.0'),
        DeclareLaunchArgument('y_pose', default_value='0.0'),
        DeclareLaunchArgument('use_sim_time', default_value='true'),
        DeclareLaunchArgument(
            'show_rviz',
            default_value='true',
            description='If true, start RViz2 with nav2_bringup default config (needs ros-humble-nav2-bringup). '
            'Set false when RViz is started separately (e.g. Docker run_task1.sh).',
        ),
        DeclareLaunchArgument(
            'start_slam',
            default_value='true',
            description='If true, start slam_toolbox online_async after the robot spawns (recommended for mapping). '
            'Set false if you launch SLAM separately (e.g. Docker run_task1.sh).',
        ),
        DeclareLaunchArgument(
            'slam_params_file',
            default_value=default_slam_params,
            description='slam_toolbox YAML (office + gz-sim lidar 6 m).',
        ),
        OpaqueFunction(function=_launch_setup),
    ])