Add Object Detection as Interface Node

rbs_perception/scripts/detector.py

@@ -7,6 +7,7 @@ Contains the following classes:
    - ModelData - High level information encapsulation
    - ObjectDetector - Greedy algorithm to build cuboids from belief maps 
 """
+# 14.06.2024 @shalenikol find_object_poses: remove "cuboid2d"
 
 import time
 
@@ -555,8 +556,8 @@ class ObjectDetector(object):
         for obj in objects:
             # Run PNP
             points = obj[1] + [(obj[0][0] * scale_factor, obj[0][1] * scale_factor)]
-            print(points)
-            cuboid2d = np.copy(points)
+            # print(points)
+            # cuboid2d = np.copy(points)
             location, quaternion, projected_points = pnp_solver.solve_pnp(points)
 
             # run multiple sample
@@ -612,7 +613,7 @@ class ObjectDetector(object):
                         "name": obj_name,
                         "location": location,
                         "quaternion": quaternion,
-                        "cuboid2d": cuboid2d,
+                        # "cuboid2d": cuboid2d,
                         "projected_points": projected_points,
                         "confidence": obj[-1],
                         "raw_points": points,

rbs_perception/scripts/pe_dope_lc.py

@@ -65,7 +65,7 @@ class PE_DOPE(Node):
         self._is_camerainfo = False
         self.topicImage = ""
         self.topicCameraInfo = ""
-        self.camera_link = ""
+        # self.camera_link = ""
         self._set_camera_topic(CAMERA_LINK_DEFAULT)
         self._sub = None
         self._sub_info = None
@@ -83,7 +83,7 @@ class PE_DOPE(Node):
         """ Service for camera name topics """
         self.topicImage = "/" + camera_link + "/image"
         self.topicCameraInfo = "/" + camera_link +"/camera_info"
-        self.camera_link = camera_link
+        # self.camera_link = camera_link
 
     def listener_camera_info(self, data):
         """ CameraInfo callback function. """
@@ -119,6 +119,10 @@ class PE_DOPE(Node):
                     dependency = par["dependency"]
         assert dependency, "no dependency"
 
+        for par in jdata["Settings"]:
+            if par["name"] == "cameraLink":
+                self._set_camera_topic(par["value"])
+
         # Create the subscribers.
         self._sub_info = self.create_subscription(CameraInfo, self.topicCameraInfo, self.listener_camera_info, 2)
         # Create the publisher.