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

def generate_launch_description():

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


    btfile_description = PathJoinSubstitution(
        [FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
    )
    btfile_param = {"bt_file_path": btfile_description}
    bt_skills_param = PathJoinSubstitution([FindPackageShare("rbs_bt_executor"), "config", "params.yaml"])

    nodes_to_start = [
        Node(
            package='behavior_tree',
            executable='bt_engine',
            # prefix=['gdbserver localhost:1234'],
            parameters=[
                btfile_param,
                bt_skills_param,
                {'use_sim_time': True}
            ],
            # arguments=[
            #     "--ros-args",
            #     "--log-level", "debug",
            # ],
        )
    ]

    return LaunchDescription(declared_arguments + nodes_to_start)