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


def construct_angle_radians(loader, node):
    """Utility function to construct radian values from yaml."""
    value = loader.construct_scalar(node)
    try:
        return float(value)
    except SyntaxError:
        raise Exception("invalid expression: %s" % value)

def construct_angle_degrees(loader, node):
    """Utility function for converting degrees into radians from yaml."""
    return math.radians(construct_angle_radians(loader, node))

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
        yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
    except Exception:
        raise Exception("yaml support not available; install python-yaml")

    try:
        with open(absolute_file_path) as file:
            return yaml.safe_load(file)
    except OSError:  # parent of IOError, OSError *and* WindowsError where available
        return None


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',
            namespace='',
            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_tree.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
            ]
        )
    ])