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"]},
#                 gripperPoints,
#                 robot_description_semantic
#             ]
#         )
#     ])


def generate_launch_description():

    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/6Dpe_test.xml')},
                {'plugins':["rbs_pose_estimation"]},
            ]
        )
    ])