feat: handle list of requests & add sensor sending/retrieval (LASR-at…

…-Home#235)

common/vision/lasr_vision_cropped_detection/examples/request.py

@@ -1,17 +1,20 @@
 #!/usr/bin/env python3
 import rospy
+from lasr_vision_msgs.msg import CDRequest
 from lasr_vision_msgs.srv import CroppedDetection, CroppedDetectionRequest
 
 
 if __name__ == "__main__":
     service = rospy.ServiceProxy("/vision/cropped_detection", CroppedDetection)
     service.wait_for_service()
     while not rospy.is_shutdown():
-        request = CroppedDetectionRequest()
-        request.method = "closest"
+        request_srv = CroppedDetectionRequest()
+        request = CDRequest()
+        request.method = "centered"
         request.use_mask = True
         request.yolo_model = "yolov8x-seg.pt"
         request.yolo_model_confidence = 0.5
         request.yolo_nms_threshold = 0.3
         request.object_names = ["person", "bottle", "chair"]
-        response = service(request)
+        request_srv.requests = [request]
+        response = service(request_srv)

common/vision/lasr_vision_cropped_detection/nodes/service.py

@@ -2,7 +2,7 @@
 import os
 import rospy
 
-from lasr_vision_cropped_detection.cropped_detection import process_detection_request
+from lasr_vision_cropped_detection.cropped_detection import process_detection_requests
 from lasr_vision_msgs.srv import (
     CroppedDetection,
     CroppedDetectionRequest,
@@ -17,7 +17,7 @@ def cropped_detection(req: CroppedDetectionRequest) -> CroppedDetectionResponse:
         if "tiago" in os.environ["ROS_MASTER_URI"]
         else "/usb_cam/image_raw"
     )
-    response: CroppedDetectionResponse = process_detection_request(
+    response: CroppedDetectionResponse = process_detection_requests(
         req, rgb_image_topic=rgb_topic
     )
     rospy.loginfo("Cropped detection request processed")

common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py

@@ -8,7 +8,12 @@
 from geometry_msgs.msg import Point, PoseWithCovarianceStamped, Polygon
 from shapely.geometry.polygon import Polygon as ShapelyPolygon
 
-from lasr_vision_msgs.msg import Detection, Detection3D
+from lasr_vision_msgs.msg import (
+    Detection,
+    Detection3D,
+    CDRequest,
+    CDResponse,
+)
 from lasr_vision_msgs.srv import (
     YoloDetection,
     YoloDetection3D,
@@ -291,20 +296,20 @@ def filter_detections_by_polygon(
     return filtered_detections, detection_polygon_ids
 
 
-def process_detection_request(
-    request: CroppedDetectionRequest,
+def process_single_detection_request(
+    request: CDRequest,
     rgb_image_topic: str = "/xtion/rgb/image_raw",
     depth_image_topic: str = "/xtion/depth_registered/points",
     yolo_2d_service_name: str = "/yolov8/detect",
     yolo_3d_service_name: str = "/yolov8/detect3d",
     robot_pose_topic: str = "/amcl_pose",
     debug_topic: str = "/lasr_vision/cropped_detection/debug",
-) -> CroppedDetectionResponse:
+) -> CDResponse:
     """Dispatches a detection request to the appropriate bounding box/mask 2D or 3D cropped
     detection function.
 
     Args:
-        request (CroppedDetectionRequest): The request to process.
+        request (CDRequest): The request to process.
         rgb_image_topic (str, optional): The topic to get an RGB image from. Defaults to "/xtion/rgb/image_raw".
         depth_image_topic (str, optional): The topic to getn an RGBD image from. Defaults to "/xtion/depth_registered/points".
         yolo_2d_service_name (str, optional): Name of the 2D Yolo detection service. Defaults to "/yolov8/detect".
@@ -313,7 +318,7 @@ def process_detection_request(
         debug_topic (str, optional): Topic to publish results to for debugging. Defaults to "/lasr_vision/cropped_detection/debug".
 
     Returns:
-        CroppedDetectionResponse: The response to the request.
+        CDResponse: The response to the request.
     """
     valid_2d_crop_methods = [
         "centered",
@@ -323,12 +328,16 @@ def process_detection_request(
         "bottom-most",
     ]
     valid_3d_crop_methods = ["closest", "furthest"]
-    response = CroppedDetectionResponse()
+    response = CDResponse()
     combined_mask = None
     if request.method in valid_2d_crop_methods:
         yolo_2d_service = rospy.ServiceProxy(yolo_2d_service_name, YoloDetection)
         yolo_2d_service.wait_for_service()
-        rgb_image = rospy.wait_for_message(rgb_image_topic, Image)
+        rgb_image = (
+            request.rgb_image
+            if request.rgb_image.data
+            else rospy.wait_for_message(rgb_image_topic, Image)
+        )
         rgb_cv2 = msg_to_cv2_img(rgb_image)
         detections = yolo_2d_service(
             rgb_image,
@@ -346,12 +355,18 @@ def process_detection_request(
                 rgb_cv2, request.method, detections
             )
         response.detections_2d = detections
+        if request.return_sensor_reading:
+            response.rgb_image = rgb_image
     elif request.method in valid_3d_crop_methods:
         yolo_3d_service = rospy.ServiceProxy(yolo_3d_service_name, YoloDetection3D)
         yolo_3d_service.wait_for_service()
         robot_pose = rospy.wait_for_message(robot_pose_topic, PoseWithCovarianceStamped)
         robot_location = robot_pose.pose.pose.position
-        pointcloud_msg = rospy.wait_for_message(depth_image_topic, PointCloud2)
+        pointcloud_msg = (
+            request.pointcloud
+            if request.pointcloud.data
+            else rospy.wait_for_message(depth_image_topic, PointCloud2)
+        )
         pointcloud_rgb = pcl_to_cv2(pointcloud_msg)
         detections = yolo_3d_service(
             pointcloud_msg,
@@ -380,6 +395,8 @@ def process_detection_request(
                 detections,
             )
         response.detections_3d = detections
+        if request.return_sensor_reading:
+            response.pointcloud = pointcloud_msg
     else:
         rospy.logerr(f"Invalid crop method: {request.method}")
         return response
@@ -429,3 +446,43 @@ def process_detection_request(
     debug_publisher.publish(cv2_img_to_msg(debug_image))
 
     return response
+
+
+def process_detection_requests(
+    request: CroppedDetectionRequest,
+    rgb_image_topic: str = "/xtion/rgb/image_raw",
+    depth_image_topic: str = "/xtion/depth_registered/points",
+    yolo_2d_service_name: str = "/yolov8/detect",
+    yolo_3d_service_name: str = "/yolov8/detect3d",
+    robot_pose_topic: str = "/amcl_pose",
+    debug_topic: str = "/lasr_vision/cropped_detection/debug",
+) -> CroppedDetectionResponse:
+    """Processes a list of detection requests.
+
+    Args:
+        request (CroppedDetectionRequestSrv): The request to process.
+        rgb_image_topic (str, optional): The topic to get an RGB image from. Defaults to "/xtion/rgb/image_raw".
+        depth_image_topic (str, optional): The topic to getn an RGBD image from. Defaults to "/xtion/depth_registered/points".
+        yolo_2d_service_name (str, optional): Name of the 2D Yolo detection service. Defaults to "/yolov8/detect".
+        yolo_3d_service_name (str, optional): Name of the 3D Yolo detection service. Defaults to "/yolov8/detect3d".
+        robot_pose_topic (str, optional): Service to get the robot's current pose. Defaults to "/amcl_pose".
+        debug_topic (str, optional): Topic to publish results to for debugging. Defaults to "/lasr_vision/cropped_detection/debug".
+
+    Returns:
+        CroppedDetectionResponseSrv: The response to the request.
+    """
+    response = CroppedDetectionResponse()
+    for req in request.requests:
+        response.responses.append(
+            process_single_detection_request(
+                req,
+                rgb_image_topic,
+                depth_image_topic,
+                yolo_2d_service_name,
+                yolo_3d_service_name,
+                robot_pose_topic,
+                debug_topic,
+            )
+        )
+
+    return response

common/vision/lasr_vision_msgs/CMakeLists.txt

@@ -49,6 +49,8 @@ add_message_files(
   Detection3D.msg
   BodyPixKeypoint.msg
   BodyPixMask.msg
+  CDRequest.msg
+  CDResponse.msg
 )
 
 ## Generate services in the 'srv' folder

common/vision/lasr_vision_msgs/msg/CDRequest.msg

@@ -0,0 +1,33 @@
+# 2D methods are "centered", "left-most" "right-most" "top-most" "bottom-most"
+# 3D methods are "closest" "furthest"
+string method
+
+# Whether to turn all pixels other than the YOLo mask to black
+# If false, uses bounding-box cropping.
+bool use_mask
+
+# Name of the objects to get
+string[] object_names
+
+# Name of the yolo model to use for detections. If using mask is True,
+# this must be a valid segementation model.
+string yolo_model
+
+# Confidence for YOLO model detections.
+float32 yolo_model_confidence
+
+# NMS threshold for YOLO model detections
+float32 yolo_nms_threshold
+
+# List of polygons to be used for 3D detections only
+geometry_msgs/Polygon[] polygons        
+
+
+# Whether to return the img or pointcloud that inference was run on
+bool return_sensor_reading
+
+# Optionally can give an RGB Image to run inference on
+sensor_msgs/Image rgb_image
+
+# Optionally can give a pointcloud to run inference on
+sensor_msgs/PointCloud2 pointcloud

common/vision/lasr_vision_msgs/msg/CDResponse.msg

@@ -0,0 +1,25 @@
+# The combined masked image, if use_mask is True, else empty.
+sensor_msgs/Image masked_img
+
+# A list of all the cropped images of detections sorted according to the
+# given method.
+sensor_msgs/Image[] cropped_imgs
+
+# A list of 2D detections, sorted to match the cropped_imgs
+lasr_vision_msgs/Detection[] detections_2d
+
+# A list of 3D detections, sorted to match the cropped_imgs
+lasr_vision_msgs/Detection3D[] detections_3d
+
+# Euclidian distance of given crop metric
+float32[] distances
+
+# IDs corresponding to which polygon(s) the detection centroid is contained in.
+# An ID of 0 corresponds to the first polygon in the request, etc.
+uint8[] polygon_ids
+
+# The RGB image used for the 2D crop
+sensor_msgs/Image rgb_image
+
+# The pointcloud used for the 3D crop
+sensor_msgs/PointCloud2 pointcloud

common/vision/lasr_vision_msgs/srv/CroppedDetection.srv

@@ -1,44 +1,6 @@
-# 2D methods are "centered", "left-most" "right-most" "top-most" "bottom-most"
-# 3D methods are "closest" "furthest"
-string method
-
-# Whether to turn all pixels other than the YOLo mask to black
-# If false, uses bounding-box cropping.
-bool use_mask
-
-# Name of the objects to get
-string[] object_names
-
-# Name of the yolo model to use for detections. If using mask is True,
-# this must be a valid segementation model.
-string yolo_model
-
-# Confidence for YOLO model detections.
-float32 yolo_model_confidence
-
-# NMS threshold for YOLO model detections
-float32 yolo_nms_threshold
-
-# List of polygons to be used for 3D detections only
-geometry_msgs/Polygon[] polygons
+# List of detection requests
+lasr_vision_msgs/CDRequest[] requests
 
 ---
-# The combined masked image, if use_mask is True, else empty.
-sensor_msgs/Image masked_img
-
-# A list of all the cropped images of detections sorted according to the
-# given method.
-sensor_msgs/Image[] cropped_imgs
-
-# A list of 2D detections, sorted to match the cropped_imgs
-lasr_vision_msgs/Detection[] detections_2d
-
-# A list of 3D detections, sorted to match the cropped_imgs
-lasr_vision_msgs/Detection3D[] detections_3d
-
-# Euclidian distance of given crop metric
-float32[] distances
-
-# IDs corresponding to which polygon(s) the detection centroid is contained in.
-# An ID of 0 corresponds to the first polygon in the request, etc.
-uint8[] polygon_ids
+# List of detection responses
+lasr_vision_msgs/CDResponse[] responses