Add minimal pick-and-place case with world embedded

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -1,7 +1,40 @@
 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():
@@ -11,25 +44,46 @@ def generate_launch_description():
         'config',
         'params.yaml'
     )
-    points = os.path.join(
-        get_package_share_directory('rbs_bt_executor'),
-        'config',
-        'points.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',
-            name="bt_engine",
-            # Do not declare a node name otherwise it messes with the action node names and will result in
-            # duplicate nodes!
+            #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']},
-                points
+                {'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
             ]
-        ),
+        )
     ])