from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
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

def generate_launch_description():

    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "bt_file",
            default_value="test_tree_env_manager.xml",
            description="BehaviorTree file for run.",
        )
    )
    bt_file = LaunchConfiguration("bt_file")

    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}

    btfile_description = PathJoinSubstitution(
        [FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
    )
    btfile_param = {"bt_file_path": btfile_description}

    nodes_to_start = [
        Node(
            package='behavior_tree',
            executable='bt_engine',
            #prefix=['xterm -e gdb -ex run --args'],
            parameters=[
                btfile_param,
                #{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/' + bt_file)},
                {'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",
                            "rbs_env_manager_starter"
                ]},
                # gripperPoints,
                robot_description_semantic
            ]
        )
    ]

    return LaunchDescription(declared_arguments + nodes_to_start)