Cartesian Move_to_pose Skill for BT Interface Node

rbss_objectdetection/scripts/od_yolo_lc.py

@@ -20,12 +20,14 @@ from rclpy.timer import Timer
 from ament_index_python.packages import get_package_share_directory
 from sensor_msgs.msg import Image
 from rbs_skill_interfaces.msg import BoundBox
+from rbs_skill_interfaces.srv import RbsBt
 
 from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
 
+CONDITION_SRV_NAME = "/condition"
 FILE_DOPE_CONFIG = "od_yolo_config.yaml"
 CAMERA_LINK_DEFAULT = "rgbd_camera"
-NODE_NAME_DEFAULT = "lc_yolo"   # the name doesn't matter in this node (defined in Launch)
+NODE_NAME_DEFAULT = "lc_yolo"
 PARAM_SKILL_CFG = "lc_yolo_cfg"
 
 class ObjectDetection(Node):
@@ -60,7 +62,7 @@ class ObjectDetection(Node):
         str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
 
         self.skill_cfg = json.loads(str_cfg)
-        self.nodeName = self.skill_cfg["Launch"]["name"]
+        self.nodeName = NODE_NAME_DEFAULT # self.skill_cfg["Module"]["node_name"] #["Launch"]["name"]
 
         self.camera_link = "" 
         self.topicImage = ""
@@ -77,6 +79,17 @@ class ObjectDetection(Node):
         self.image_det = []
         self.bbox_res = None
 
+        self._srv_condition = self.create_service(RbsBt, NODE_NAME_DEFAULT + CONDITION_SRV_NAME, self.condition_callback)
+        # , callback_group=self.cb_group)
+
+    def condition_callback(self, request, response):
+        # is_success = self._interface(request.sid, request.action, request.command)
+        _is = False
+        if request.command == "isDetectionRun":
+            _is = True
+        response.ok = _is
+        return response
+
     def _set_camera_topic(self, camera_link: str):
         """ Service for camera name topics """
         self.camera_link = camera_link