11-03 fixes (#145)

* fix: catkin_python_setup().

* fix: typehinting.

* fix: use global tf buffer, use PointStamped.

* chore: update exec dependencies.

* fix: correct list comprehension to consider new type (YoloDetection3D).

common/helpers/markers/CMakeLists.txt

@@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##

common/helpers/markers/src/markers/__init__.py

@@ -5,6 +5,8 @@
 
 from collections import defaultdict
 
+from typing import Union
+
 publisher_counts = defaultdict(int)
 
 
@@ -36,7 +38,7 @@ def create_marker(
 def create_and_publish_marker(
     publisher: rospy.Publisher,
     point_stamped: PointStamped,
-    idx: int | None = None,
+    idx: Union[int, None] = None,
     r: float = 0.0,
     g: float = 1.0,
     b: float = 0.0,

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/package.xml

@@ -52,6 +52,8 @@
   <build_depend>catkin_virtualenv</build_depend>
   <depend>lasr_vision_msgs</depend>
   <exec_depend>markers</exec_depend>
+  <exec_depend>cv2_img</exec_depend>
+  <exec_depend>cv2_pcl</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

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)
 

skills/src/lasr_skills/detect_3d_in_area.py

@@ -22,17 +22,19 @@ def __init__(self, area_polygon: Polygon):
             self.area_polygon = area_polygon
 
         def execute(self, userdata):
+            detected_objects = userdata["detections_3d"].detected_objects
+
             satisfied_points = [
-                self.area_polygon.contains(Point(pose[0], pose[1]))
-                for (_, pose) in userdata.detections_3d
+                self.area_polygon.contains(Point(object.point.x, object.point.y))
+                for object in detected_objects
             ]
             filtered_detections = [
-                userdata.detections_3d[i]
-                for i in range(0, len(userdata.detections_3d))
+                detected_objects[i]
+                for i in range(0, len(detected_objects))
                 if satisfied_points[i]
             ]
 
-            userdata.detections_3d = filtered_detections
+            userdata["detections_3d"] = filtered_detections
             return "succeeded"
 
     def __init__(