from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration

import os
import ast
import json
from ament_index_python.packages import get_package_share_directory


share_dir = get_package_share_directory('vision_detect')


def generate_launch_description():
    args_detect = [
        DeclareLaunchArgument('output_boxes', default_value='True'),
        DeclareLaunchArgument('output_masks', default_value='False'),
        DeclareLaunchArgument('color_image_topic', default_value='/camera/color/image_raw'),
        DeclareLaunchArgument('depth_image_topic', default_value='/camera/depth/image_raw'),
        DeclareLaunchArgument('camera_info_topic', default_value='/camera/color/camera_info'),
    ]

    def create_detect_node(context):
        output_boxes = LaunchConfiguration('output_boxes').perform(context)
        output_masks = LaunchConfiguration('output_masks').perform(context)
        color_image_topic = LaunchConfiguration('color_image_topic').perform(context)
        depth_image_topic = LaunchConfiguration('depth_image_topic').perform(context)
        camera_info_topic = LaunchConfiguration('camera_info_topic').perform(context)

        return [
            Node(
                package='vision_detect',
                executable='crossboard_detect_node',
                parameters=[{
                    'output_boxes': output_boxes.lower() == 'true',
                    'output_masks': output_masks.lower() == 'true',
                    'color_image_topic': color_image_topic,
                    'depth_image_topic': depth_image_topic,
                    'camera_info_topic': camera_info_topic,
                }]
            )
        ]

    return LaunchDescription(args_detect + [
        OpaqueFunction(function=create_detect_node),
    ])
