# Copyright (c) 2018 Intel Corporation
#
# 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.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('kohm_gazebo')

    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('config')
    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
    map_subscribe_transient_local = LaunchConfiguration(
        'map_subscribe_transient_local')

    lifecycle_nodes = [
        'controller_server', 'planner_server', 'recoveries_server',
        'bt_navigator', 'waypoint_follower'
    ]

    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/cmd_vel', '/nav_vel'), ('/odom', '/kohm/odom')]

    param_substitutions = {
        'use_sim_time': use_sim_time,
        'default_bt_xml_filename': default_bt_xml_filename,
        'autostart': autostart,
        'map_subscribe_transient_local': map_subscribe_transient_local
    }

    configured_params = RewrittenYaml(source_file=params_file,
                                      root_key=namespace,
                                      param_rewrites=param_substitutions,
                                      convert_types=True)

    return LaunchDescription([
        # Set env var to print messages to stdout immediately
        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
        DeclareLaunchArgument('namespace',
                              default_value='',
                              description='Top-level namespace'),
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument(
            'autostart',
            default_value='true',
            description='Automatically startup the nav2 stack'),
        DeclareLaunchArgument(
            'config',
            default_value=os.path.join(bringup_dir, 'config/navigation',
                                       'nav2_params.yaml'),
            description='Full path to the ROS2 parameters file to use'),
        DeclareLaunchArgument(
            'default_bt_xml_filename',
            default_value=os.path.join(
                get_package_share_directory('nav2_bt_navigator'),
                'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
            description='Full path to the behavior tree xml file to use'),
        DeclareLaunchArgument(
            'map_subscribe_transient_local',
            default_value='false',
            description='Whether to set the map subscriber QoS to transient local'),
        Node(package='nav2_controller',
             executable='controller_server',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),
        Node(package='nav2_planner',
             executable='planner_server',
             name='planner_server',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),
        Node(package='nav2_recoveries',
             executable='recoveries_server',
             name='recoveries_server',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),
        Node(package='nav2_bt_navigator',
             executable='bt_navigator',
             name='bt_navigator',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),
        Node(package='nav2_waypoint_follower',
             executable='waypoint_follower',
             name='waypoint_follower',
             output='screen',
             parameters=[configured_params],
             remappings=remappings),
        Node(package='nav2_lifecycle_manager',
             executable='lifecycle_manager',
             name='lifecycle_manager_navigation',
             output='screen',
             parameters=[{
                 'use_sim_time': use_sim_time
             }, {
                 'autostart': autostart
             }, {
                 'node_names': lifecycle_nodes
             }]),
    ])
