#!/usr/bin/python3
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():
    debug = False
    respawn = False

    default_config_file = os.path.join(
        get_package_share_directory("yoso_pkg"), "config", "panoptic_config.yaml"
    )

    node_name = LaunchConfiguration("node_name")
    config_file = LaunchConfiguration("config_file")

    declare_config_file_cmd = DeclareLaunchArgument(
        "config_file",
        default_value=default_config_file,
        description="panoptic seg module parameters structured in a .yaml file.",
    )

    declare_node_name_cmd = DeclareLaunchArgument(
        "node_name",
        default_value="panoptic_yoso_node",
        description="Name of the wrapper node.",
    )

    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "[{time}] [{name}] [{severity}] {message}"
    )

    # see https://navigation.ros.org/tutorials/docs/get_backtrace.html
    launch_prefix = ["xterm -e gdb -ex run --args"] if debug else ""

    # node
    panoptic_seg_node = Node(
        package="yoso_pkg",
        # namespace='yoso',
        executable="yoso_inferencer",
        name=node_name,
        output="screen",
        respawn=respawn,
        emulate_tty=True,
        prefix=launch_prefix,
        parameters=[config_file],
    )

    # Define LaunchDescription variable and return it
    ld = LaunchDescription()

    ld.add_action(declare_node_name_cmd)
    ld.add_action(declare_config_file_cmd)
    ld.add_action(panoptic_seg_node)

    return ld