#!/usr/bin/env python3
#
# Copyright 2019 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Joep Tool
# Migrated from Gazebo Classic to Gazebo Sim (gz-sim / Harmonic)

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    x_pose = LaunchConfiguration('x_pose', default='0.0')
    y_pose = LaunchConfiguration('y_pose', default='0.0')

    pkg_share_dir = get_package_share_directory('turtlebot3_gazebo')
    world = os.path.join(pkg_share_dir, 'worlds', 'office_gz_dartsim.sdf')
    models_path = os.path.join(pkg_share_dir, 'models')
    office_models_path = os.path.join(models_path, 'turtlebot3_office')

    # Headless mode: no GUI, no RVIZ
    # gui_config = os.path.join(pkg_share_dir, 'gui', 'office_gui.config')
    # gz_args = f'-r {world} --gui-config {gui_config}'
    gz_args = f'-r -s {world}'
    # gz_args = f'-r {world}'

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

    robot_state_publisher_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
        ),
        launch_arguments={'use_sim_time': use_sim_time}.items()
    )

    spawn_turtlebot_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
        ),
        launch_arguments={
            'x_pose': x_pose,
            'y_pose': y_pose
        }.items()
    )

    # Fix LaserScan frame_id:
    #   bridge publishes on /scan_raw (frame_id = 'burger/base_scan/hls_lfcd_lds')
    #   this node republishes on /scan with corrected frame_id = 'base_scan'
    scan_frame_fix_cmd = Node(
        package='turtlebot3_gazebo',
        executable='scan_frame_fix.py',
        output='screen',
        parameters=[{'use_sim_time': True}],
    )

    ld = LaunchDescription()

    # Set GZ_SIM_RESOURCE_PATH so gz sim can resolve model:// URIs
    ld.add_action(SetEnvironmentVariable(
        name='GZ_SIM_RESOURCE_PATH',
        value=office_models_path + os.pathsep + models_path
    ))

    # Add the commands to the launch description

    ld.add_action(gz_sim_cmd)
    ld.add_action(robot_state_publisher_cmd)
    ld.add_action(spawn_turtlebot_cmd)
    ld.add_action(scan_frame_fix_cmd)
    return ld