Migration to BT.cpp v4 & new BT executor

rbs_utils/rbs_utils/CMakeLists.txt

@@ -30,6 +30,13 @@ set(deps
 
 add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
 
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/assembly_config_service.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(
   DIRECTORY include/
   DESTINATION include

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -0,0 +1,85 @@
+#!/usr/bin/python3
+
+import os
+import rclpy
+from rclpy.node import Node
+import yaml
+from geometry_msgs.msg import Point, Pose, Quaternion, PoseArray
+from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
+from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
+
+class AssemblyConfigService(Node):
+    def __init__(self, node_name="assembly_config"):
+        super().__init__(node_name)
+
+        self.declare_parameter("db_folder", "asp-example")
+        db_folder = self.get_parameter("db_folder").get_parameter_value().string_value
+        
+        # Parse the YAML file
+        yaml_file = os.path.join(os.getenv('RBS_ASSEMBLY_DIR', ''), db_folder,'rbs_db.yaml')
+        self.assembly_config = parse_yaml(yaml_file)
+        
+        # Create services
+        self.workspace_service = self.create_service(GetWorkspace, "get_workspace", self.get_workspace_callback)
+        self.grasp_pose_service = self.create_service(GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback)
+
+    def get_workspace_callback(self, request, response):
+        response.workspace = self.assembly_config.workspace
+        response.ok = True
+        return response
+
+    def get_grasp_pose_callback(self, request, response):
+        response.grasp_pose = self.assembly_config.grasp_pose
+        response.ok = True
+        return response
+
+def parse_yaml(yaml_file):
+    with open(yaml_file, 'r') as file:
+        data = yaml.safe_load(file)
+    
+    assets_db = data.get('assets_db', '')
+
+    workspace = [Point(**point) for point in data.get('workspace', [])]
+
+    absolute_part = []
+    for part in data.get('absolute_part', []):
+        position = part['pose']['position']
+        orientation = part['pose']['orientation']
+        pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
+        absolute_part.append(NamedPose(name=part['name'], pose=pose))
+
+    relative_part = []
+    for part in data.get('relative_part', []):
+        position = part['pose']['position']
+        orientation = part['pose']['orientation']
+        pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
+        relative_part.append(RelativeNamedPose(name=part['name'], relative_at=part['relative_at'], pose=pose))
+
+    grasp_pose = []
+    for pose in data.get('grasp_pose', []):
+        position = pose['pose']['position']
+        orientation = pose['pose']['orientation']
+        pose_obj = Pose(position=Point(**position), orientation=Quaternion(**orientation))
+        grasp_pose.append(RelativeNamedPose(name=pose['name'], relative_at=pose['relative_at'], pose=pose_obj))
+
+    extra_poses = []
+    
+    assembly_config = AssemblyConfig(
+        assets_db=assets_db,
+        workspace=workspace,
+        absolute_part=absolute_part,
+        relative_part=relative_part,
+        grasp_pose=grasp_pose,
+        extra_poses=extra_poses
+    )
+    
+    return assembly_config
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = AssemblyConfigService()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

rbs_utils/rbs_utils_interfaces/CMakeLists.txt

@@ -15,6 +15,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/AssemblyConfig.msg"
     "msg/NamedPose.msg"
     "msg/RelativeNamedPose.msg"
+    "srv/GetGraspPose.srv"
+    "srv/GetWorkspace.srv"
     DEPENDENCIES std_msgs geometry_msgs
 )
 

rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv

@@ -0,0 +1,4 @@
+string model_name
+---
+rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
+bool ok

rbs_utils/rbs_utils_interfaces/srv/GetWorkspace.srv

@@ -0,0 +1,4 @@
+string type
+---
+geometry_msgs/Point[] workspace
+bool ok