import os
from launch_ros.actions import Node
from ur_moveit_config.launch_common import load_yaml
from launch import LaunchDescription

def generate_launch_description():
    declared_arguments = []
    
    points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")

    grasp_pose_loader = Node(
        package="rbs_skill_servers",
        executable="pick_place_pose_loader_service_server",
        output="screen",
        emulate_tty=True,
        parameters=[
            points_params
        ]
    )
    pose_estimation = Node(
        package="rbs_perception",
        executable="pose_estimation_lifecycle.py",
    )

    nodes_to_start = [
        grasp_pose_loader,
        pose_estimation
    ]
    return LaunchDescription(declared_arguments + nodes_to_start)