#!/usr/bin/env python3
# Copyright 2020 Gaitech Korea 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.

# Author: Brighten Lee

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    default_config_locks = os.path.join(get_package_share_directory('twist_mux'),
                                        'config', 'twist_mux_locks.yaml')
    default_config_topics = os.path.join(get_package_share_directory('twist_mux'),
                                         'config', 'twist_mux_topics.yaml')
    default_config_joystick = os.path.join(get_package_share_directory('twist_mux'),
                                           'config', 'joystick.yaml')

    return LaunchDescription([
        DeclareLaunchArgument(
            'config_locks',
            default_value=default_config_locks,
            description='Default locks config file'),
        DeclareLaunchArgument(
            'config_topics',
            default_value=default_config_topics,
            description='Default topics config file'),
        DeclareLaunchArgument(
            'config_joy',
            default_value=default_config_joystick,
            description='Default joystick config file'),
        DeclareLaunchArgument(
            'cmd_vel_out',
            default_value='twist_mux/cmd_vel',
            description='cmd vel output topic'),
        Node(
            package='twist_mux',
            executable='twist_mux',
            output='screen',
            remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))},
            parameters=[
                LaunchConfiguration('config_locks'),
                LaunchConfiguration('config_topics'),
                LaunchConfiguration('config_joy')]
        ),

        Node(
            package='twist_mux',
            executable='twist_marker',
            output='screen',
            remappings={('/twist', LaunchConfiguration('cmd_vel_out'))},
            parameters=[{
                'frame_id': 'base_link',
                'scale': 1.0,
                'vertical_position': 2.0}])
            ])

[((965, 1005), 'ament_index_python.packages.get_package_share_directory', 'get_package_share_directory', (['"""twist_mux"""'], {}), "('twist_mux')\n", (992, 1005), False, 'from ament_index_python.packages import get_package_share_directory\n'), ((1122, 1162), 'ament_index_python.packages.get_package_share_directory', 'get_package_share_directory', (['"""twist_mux"""'], {}), "('twist_mux')\n", (1149, 1162), False, 'from ament_index_python.packages import get_package_share_directory\n'), ((1283, 1323), 'ament_index_python.packages.get_package_share_directory', 'get_package_share_directory', (['"""twist_mux"""'], {}), "('twist_mux')\n", (1310, 1323), False, 'from ament_index_python.packages import get_package_share_directory\n'), ((1435, 1553), 'launch.actions.DeclareLaunchArgument', 'DeclareLaunchArgument', (['"""config_locks"""'], {'default_value': 'default_config_locks', 'description': '"""Default locks config file"""'}), "('config_locks', default_value=default_config_locks,\n    description='Default locks config file')\n", (1456, 1553), False, 'from launch.actions import DeclareLaunchArgument\n'), ((1596, 1717), 'launch.actions.DeclareLaunchArgument', 'DeclareLaunchArgument', (['"""config_topics"""'], {'default_value': 'default_config_topics', 'description': '"""Default topics config file"""'}), "('config_topics', default_value=default_config_topics,\n    description='Default topics config file')\n", (1617, 1717), False, 'from launch.actions import DeclareLaunchArgument\n'), ((1760, 1882), 'launch.actions.DeclareLaunchArgument', 'DeclareLaunchArgument', (['"""config_joy"""'], {'default_value': 'default_config_joystick', 'description': '"""Default joystick config file"""'}), "('config_joy', default_value=default_config_joystick,\n    description='Default joystick config file')\n", (1781, 1882), False, 'from launch.actions import DeclareLaunchArgument\n'), ((1925, 2036), 'launch.actions.DeclareLaunchArgument', 'DeclareLaunchArgument', (['"""cmd_vel_out"""'], {'default_value': '"""twist_mux/cmd_vel"""', 'description': '"""cmd vel output topic"""'}), "('cmd_vel_out', default_value='twist_mux/cmd_vel',\n    description='cmd vel output topic')\n", (1946, 2036), False, 'from launch.actions import DeclareLaunchArgument\n'), ((2303, 2338), 'launch.substitutions.LaunchConfiguration', 'LaunchConfiguration', (['"""config_locks"""'], {}), "('config_locks')\n", (2322, 2338), False, 'from launch.substitutions import LaunchConfiguration\n'), ((2356, 2392), 'launch.substitutions.LaunchConfiguration', 'LaunchConfiguration', (['"""config_topics"""'], {}), "('config_topics')\n", (2375, 2392), False, 'from launch.substitutions import LaunchConfiguration\n'), ((2410, 2443), 'launch.substitutions.LaunchConfiguration', 'LaunchConfiguration', (['"""config_joy"""'], {}), "('config_joy')\n", (2429, 2443), False, 'from launch.substitutions import LaunchConfiguration\n'), ((2224, 2258), 'launch.substitutions.LaunchConfiguration', 'LaunchConfiguration', (['"""cmd_vel_out"""'], {}), "('cmd_vel_out')\n", (2243, 2258), False, 'from launch.substitutions import LaunchConfiguration\n'), ((2607, 2641), 'launch.substitutions.LaunchConfiguration', 'LaunchConfiguration', (['"""cmd_vel_out"""'], {}), "('cmd_vel_out')\n", (2626, 2641), False, 'from launch.substitutions import LaunchConfiguration\n')]