feat: late night deat detections

common/helpers/tf_pcl/CMakeLists.txt

@@ -16,7 +16,7 @@ find_package(catkin REQUIRED)
 ## 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/tf_pcl/src/tf_pcl/__init__.py

@@ -51,5 +51,7 @@ def pcl_transform(pcl: PointCloud2, transform: TransformStamped) -> PointCloud2:
     pcl_arr["y"] = transformed_pcl[1]
     pcl_arr["z"] = transformed_pcl[2]
 
-    transformed_pcl = rnp.point_cloud2.array_to_pointcloud2(pcl_arr)
+    transformed_pcl = rnp.point_cloud2.array_to_pointcloud2(
+        pcl_arr, stamp=pcl.header.stamp, frame_id=transform.child_frame_id
+    )
     return transformed_pcl

common/vision/lasr_vision_cropped_detection/examples/request.py

@@ -10,11 +10,11 @@
     while not rospy.is_shutdown():
         request_srv = CroppedDetectionRequest()
         request = CDRequest()
-        request.method = "centered"
+        request.method = "closest"
         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"]
+        request.object_names = ["person"]
         request_srv.requests = [request]
         response = service(request_srv)

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

@@ -3,10 +3,11 @@
 import numpy as np
 import cv2
 import rospy
-
+from shapely.validation import explain_validity
 from sensor_msgs.msg import Image, PointCloud2
 from geometry_msgs.msg import Point, PoseWithCovarianceStamped, Polygon
 from shapely.geometry.polygon import Polygon as ShapelyPolygon
+from shapely.geometry.point import Point as ShapelyPoint
 
 from lasr_vision_msgs.msg import (
     Detection,
@@ -58,9 +59,6 @@ def _2d_bbox_crop(
     else:
         raise ValueError(f"Invalid 2D crop_method: {crop_method}")
 
-    if len(detections) == 0:
-        raise ValueError("No detections found")
-
     distances = [
         np.sqrt((x_to_compare - det.xywh[0]) ** 2 + (y_to_compare - det.xywh[1]) ** 2)
         for det in detections
@@ -120,12 +118,6 @@ def _2d_mask_crop(
     else:
         raise ValueError(f"Invalid 2D crop_method: {crop_method}")
 
-    if len(detections) == 0:
-        raise ValueError("No detections found")
-
-    if len(detections[0].xyseg) == 0:
-        raise ValueError("No segmentation found")
-
     distances = [
         np.sqrt((x_to_compare - det.xywh[0]) ** 2 + (y_to_compare - det.xywh[1]) ** 2)
         for det in detections
@@ -171,9 +163,6 @@ def _3d_bbox_crop(
         List[np.ndarray]: List of cropped images.
     """
 
-    if len(detections) == 0:
-        raise ValueError("No detections found")
-
     distances = [
         np.sqrt(
             (robot_location.x - det.point.x) ** 2
@@ -228,8 +217,6 @@ def _3d_mask_crop(
         Tuple[List[np.ndarray], np.ndarray, List[Detection3D]]: Tuple of cropped images, the combined mask, and the detections.
     """
 
-    if len(detections) == 0:
-        raise ValueError("No detections found")
     distances = [
         np.sqrt(
             (robot_location.x - det.point.x) ** 2
@@ -288,8 +275,16 @@ def filter_detections_by_polygon(
     filtered_detections: List[Detection3D] = []
     for index, polygon in enumerate(polygons):
         area_polygon = ShapelyPolygon([(point.x, point.y) for point in polygon.points])
+        print(f"Area polygon: {area_polygon}")
+        print(f"Polygon Area: {area_polygon.area}")
+        print(f"Polygon is valid: {area_polygon.is_valid}")
+        print(explain_validity(area_polygon))
         for detection in detections:
-            if area_polygon.contains(Point(detection.point.x, detection.point.y)):
+            print(f"Point: {detection.point}")
+            if area_polygon.contains(
+                ShapelyPoint(detection.point.x, detection.point.y)
+            ):
+                print(f"Detection {detection} is within polygon {index}")
                 detection_polygon_ids.append(index)
                 filtered_detections.append(detection)
 
@@ -406,6 +401,7 @@ def process_single_detection_request(
     ]
 
     debug_publisher = rospy.Publisher(debug_topic, Image, queue_size=10)
+    closest_pub = rospy.Publisher(debug_topic + "_closest", Image, queue_size=10)
     combined_mask_debug_publisher = rospy.Publisher(
         debug_topic + "_mask", Image, queue_size=10
     )
@@ -427,23 +423,29 @@ def process_single_detection_request(
         combined_mask_debug_publisher.publish(cv2_img_to_msg(combined_mask))
         response.masked_img = cv2_img_to_msg(combined_mask)
 
+    try:
+        closest_pub.publish(cv2_img_to_msg(cropped_images[0]))
+    except IndexError:
+        rospy.logwarn("No detections found")
     response.distances = distances
+    try:
+        debug_image = np.hstack(cropped_images)
+        # Add distances to the image
+        for i, dist in enumerate(distances):
+            cv2.putText(
+                debug_image,
+                f"Dist: {round(dist, 2)}",
+                (i * cropped_images[0].shape[0] + 150, 50),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                1,
+                (0, 255, 0),
+                2,
+                cv2.LINE_AA,
+            )
 
-    debug_image = np.hstack(cropped_images)
-    # Add distances to the image
-    for i, dist in enumerate(distances):
-        cv2.putText(
-            debug_image,
-            f"Dist: {round(dist, 2)}",
-            (i * cropped_images[0].shape[0] + 150, 50),
-            cv2.FONT_HERSHEY_SIMPLEX,
-            1,
-            (0, 255, 0),
-            2,
-            cv2.LINE_AA,
-        )
-
-    debug_publisher.publish(cv2_img_to_msg(debug_image))
+        debug_publisher.publish(cv2_img_to_msg(debug_image))
+    except ValueError:
+        rospy.logwarn("No detections found")
 
     return response
 

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

@@ -110,10 +110,12 @@ def detect_3d(
     img = cv2_pcl.pcl_to_cv2(request.pcl)
 
     # transform pcl to map frame
-    trans = tf_buffer.lookup_transform(
-        "map", request.pcl.header.frame_id, rospy.Time(0), rospy.Duration(1.0)
-    )
-    pcl_map = do_transform_cloud(request.pcl, trans)
+    # trans = tf_buffer.lookup_transform(
+    #     "map", request.pcl.header.frame_id, rospy.Time(0), rospy.Duration(1.0)
+    # )
+    # pcl_map = do_transform_cloud(request.pcl, trans)
+    print(request.pcl.header.frame_id)
+    pcl_map = request.pcl
 
     # load model
     rospy.loginfo("Loading model")
@@ -146,6 +148,9 @@ def detect_3d(
                 width=request.pcl.width,
             )
             detection.point = Point(*centroid)
+            rospy.loginfo(
+                f"Detected point: {detection.point} of object {detection.name}"
+            )
 
         if debug_point_publisher is not None:
             markers.create_and_publish_marker(

skills/src/lasr_skills/look_to_point.py

@@ -46,7 +46,7 @@ def execute(self, userdata):
         self.client.send_goal(goal)
 
         # Wait for the result with a timeout of 7 seconds
-        finished_within_time = self.client.wait_for_result(rospy.Duration(7.0))
+        finished_within_time = self.client.wait_for_result(rospy.Duration(2.0))
 
         if finished_within_time:
             state = self.client.get_state()

tasks/receptionist/src/receptionist/state_machine.py

@@ -2,6 +2,7 @@
 import smach
 import smach_ros
 
+from lasr_vision_msgs.srv import Recognise
 from geometry_msgs.msg import Pose
 from shapely.geometry import Polygon
 from lasr_skills import (
@@ -17,6 +18,8 @@
     FindAndLookAt,
     HandleGuest,
     PointCloudSweep,
+    RunAndProcessDetections,
+    RecognisePeople,
 )
 
 
@@ -489,16 +492,62 @@ def _compute_person_point(self, userdata):
         """
         pass
 
-    def _detect_and_sweep(
+    def _detect_sweep_recognise_seat(
         self,
         expected_seated_guests: int,
     ):
 
+        # Looks to a list of given points in the seating area and obtains the transformed
+        # pointclouds
         smach.StateMachine.add(
-            f"DETECT_AND_SWEEP_{expected_seated_guests}",
+            f"SWEEP_{expected_seated_guests}",
             PointCloudSweep(self.sweep_points),
             transitions={
                 "succeeded": f"RUN_AND_PROCESS_DETECTIONS_{expected_seated_guests}",
                 "failed": "failed",
             },
         )
+
+        # For each pointcloud, runs the 3D masked cropped detection service, looking for chairs
+        # and people, and removes any duplicate detections, and return empty chairs by removing
+        # any chair detections that are over people.
+        smach.StateMachine.add(
+            f"RUN_AND_PROCESS_DETECTIONS_{expected_seated_guests}",
+            RunAndProcessDetections(seating_area=self.seat_area),
+            transitions={
+                "succeeded": f"RECOGNISE_{expected_seated_guests}",
+                "failed": "failed",
+            },
+        )
+
+        smach.StateMachine.add(
+            f"RECOGNISE_{expected_seated_guests}",
+            RecognisePeople(self.userdata.dataset),
+            transitions={
+                "succeeded": f"LOOK_AT_SEAT_{expected_seated_guests}",
+                "failed": "failed",
+            },
+        )
+
+        # Look at guest or host
+        smach.StateMachine.add(
+            f"GET_POINTSTAMPED_{expected_seated_guests}",
+            PointCloudSweep.GetPointStamped(),
+            transitions={
+                "succeeded": f"LOOK_AT_SEAT_{expected_seated_guests}",
+                "failed": "failed",
+            },
+        )
+
+        smach.StateMachine.add(
+            f"LOOK_AT_SEAT_{expected_seated_guests}",
+            LookToPoint(pointstamped=None),
+            transitions={
+                "succeeded": f"GET_POINTSTAMPED_{expected_seated_guests}",
+                "failed": "failed",
+            },
+        )
+
+        # Handle sofas.
+
+        # Seat guest.

tasks/receptionist/src/receptionist/states/__init__.py

@@ -8,3 +8,5 @@
 from .get_name_or_drink import ParseTranscribedInfo
 from .handle_guest import HandleGuest
 from .pointcloud_sweep import PointCloudSweep
+from .run_and_process_detections import RunAndProcessDetections
+from .recognise_people import RecognisePeople

tasks/receptionist/src/receptionist/states/pointcloud_sweep.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
-from typing import List, Tuple
+from typing import List, Tuple, Optional
 import smach
 import rospy
 import tf2_ros as tf
 
+from tf_pcl import pcl_transform
 from std_msgs.msg import Header
 from geometry_msgs.msg import PointStamped, Point
 from sensor_msgs.msg import PointCloud2
-from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
 
 from lasr_skills import LookToPoint
 
@@ -48,7 +48,7 @@ def __init__(self, sweep_points: List[Tuple[float, float, float]]):
                     transitions={
                         "succeeded": f"GetTransformedPointcloud_{index}",
                         "aborted": "failed",
-                        "timed_out": "failed",
+                        "timed_out": f"GetTransformedPointcloud_{index}",
                     },
                     remapping={
                         "pointstamped": f"pointstamped_{index}",
@@ -93,27 +93,31 @@ def execute(self, userdata):
                     rospy.Time(0),
                     rospy.Duration(1.0),
                 )
-                pcl_map = do_transform_cloud(pcl, trans)
+                pcl_map = pcl_transform(pcl, trans)
                 userdata.transformed_pointclouds.append(pcl_map)
             except Exception as e:
                 rospy.logerr(f"Failed to get and transform pointcloud: {str(e)}")
                 return "failed"
             return "succeeded"
 
     class GetPointStamped(smach.State):
-        def __init__(self, point: Tuple[float, float, float]):
+        def __init__(self, point: Optional[Tuple[float, float, float]]):
             smach.State.__init__(
                 self,
                 outcomes=["succeeded", "failed"],
-                input_keys=["point"],
+                input_keys=["point"] if point is None else [],
                 output_keys=["pointstamped"],
             )
             self.point = point
 
         def execute(self, userdata):
             try:
+                if self.point is None:
+                    point = userdata.point
+                else:
+                    point = self.point
                 pointstamped = PointStamped(
-                    point=Point(self.point[0], self.point[1], self.point[2]),
+                    point=Point(point[0], point[1], point[2]),
                     header=Header(frame_id="map"),
                 )
                 userdata.pointstamped = pointstamped

tasks/receptionist/src/receptionist/states/recognise_people.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+import smach
+import rospy
+from lasr_vision_msgs.srv import Recognise, RecogniseRequest
+
+
+class RecognisePeople(smach.State):
+    def __init__(
+        self,
+        dataset: str,
+        confidence: float = 0.5,
+    ):
+        smach.State.__init__(
+            self,
+            outcomes=["succeeded", "failed"],
+            output_keys=["named_detections"],
+            input_keys=["people_detections"],
+        )
+        self._dataset = dataset
+        self._confidence = confidence
+        self._recognise = rospy.ServiceProxy("/recognise", Recognise)
+
+    def execute(self, userdata):
+        try:
+            named_detections = []
+            for person_detection in userdata.people_detections:
+                img_msg = person_detection[1]
+                result = self._recognise(img_msg, self._dataset, self._confidence)
+                named_detections.append(
+                    [person_detection[0], result.detections[0].name]
+                )
+            userdata.named_detections = named_detections
+        except rospy.ServiceException as e:
+            rospy.logwarn(f"Unable to perform inference. ({str(e)})")
+            return "failed"
+        return "succeeded"