fix: use global tf buffer, use PointStamped.

common/vision/lasr_vision_yolov8/nodes/service

@@ -79,6 +79,7 @@ def detect_3d(request: YoloDetection3DRequest) -> YoloDetection3DResponse:
     return yolo.detect_3d(request, debug_inference_publisher, debug_point_publisher)
 
 
+yolo.start_tf_buffer()
 rospy.Service("/yolov8/detect", YoloDetection, detect)
 rospy.Service("/yolov8/detect3d", YoloDetection3D, detect_3d)
 rospy.loginfo("YOLOv8 service started")

common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/__init__.py

@@ -1 +1 @@
-from .yolo import detect, detect_3d, load_model
+from .yolo import detect, detect_3d, load_model, start_tf_buffer

common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py

@@ -20,6 +20,13 @@
 import tf2_ros as tf
 import tf2_geometry_msgs  # noqa
 
+# global tf buffer
+tf_buffer = tf.Buffer(cache_time=rospy.Duration(10))
+
+
+def start_tf_buffer() -> None:
+    tf.TransformListener(tf_buffer)
+
 
 # model cache
 loaded_models = {}
@@ -96,7 +103,6 @@ def detect_3d(
     """
     Run YOLO 3D inference on given detection request
     """
-    tf_buffer = tf.Buffer()
 
     # Extract rgb image from pointcloud
     rospy.loginfo("Decoding")
@@ -128,25 +134,24 @@ def detect_3d(
 
             centroid = cv2_pcl.seg_to_centroid(request.pcl, np.array(detection.xyseg))
 
-            point = Point(*centroid)
             point_stamped = PointStamped()
-            point_stamped.point = point
+            point_stamped.point = Point(*centroid)
             point_stamped.header.frame_id = request.pcl.header.frame_id
             point_stamped.header.stamp = rospy.Time(0)
 
             # TODO: handle tf errors properly
             while not rospy.is_shutdown():
                 try:
-                    detection.point = tf_buffer.transform(point_stamped, "map").point
+                    point_stamped = tf_buffer.transform(point_stamped, "map")
+                    detection.point = point_stamped.point
                     break
-                except Exception:
+                except Exception as e:
+                    rospy.logerr(e)
                     continue
 
             # publish to debug topic
             if debug_point_publisher is not None:
-                markers.create_and_publish_marker(
-                    debug_point_publisher, detection.point
-                )
+                markers.create_and_publish_marker(debug_point_publisher, point_stamped)
 
         detected_objects.append(detection)