from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory 
import os
import yaml
import math
from rbs_launch_utils.launch_common import load_yaml


def generate_launch_description():

    # bt_config = os.path.join(
    #     get_package_share_directory('rbs_bt_executor'),
    #     'config',
    #     'params.yaml'
    # )
    # points = load_yaml(
    #     package_name="rbs_bt_executor",
    #     file_path="config/points.yaml"
    # )

    gripperPoints = load_yaml(
        package_name="rbs_bt_executor",
        file_path="config/gripperPositions.yaml")

    robot_description_semantic_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
            ),
            " ",
            "name:=",
            "ur",
            " ",
            "prefix:=",
            "",
            " ",
        ]
    )
    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
    
    

    return LaunchDescription([
        Node(
            package='behavior_tree',
            executable='bt_engine',
            #prefix=['xterm -e gdb -ex run --args'],
            parameters=[
                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
                {'plugins':["rbs_skill_move_topose_bt_action_client",
                            "rbs_skill_get_pick_place_pose_service_client",
                            "rbs_skill_gripper_move_bt_action_client",
                            "rbs_skill_move_joint_state",
                            "rbs_add_planning_scene_object",
                            "rbs_pose_estimation"
                ]},
                # gripperPoints,
                robot_description_semantic
            ]
        )
    ])