Add experimental implementation for sorting objects by distance

src/homer_gesture_recognition/interfaces.py

@@ -7,6 +7,10 @@ from homer_utils.ros_utils import is_topic_advertised
 from homer_utils.msg_wrapper import PointWrapper
 from homer_utils.visualization_utils import publish_marker
 
+from tf2_ros import LookupException, ExtrapolationException
+from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
+from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point, do_transform_vector3
+
 LOG_PREFIX = "[GESTURE_INTERFACE] "
 
 
@@ -203,9 +207,42 @@ class GestureRecognitionInterface:
         :returns: List of `homer_object_detection_msgs/Object`
         """
 
-        # TODO:
-        # get pointing ray if none provided
-        # transform objects to map
-        # order by distance to ray
-        # return closest
-        pass
+        def ray_distance(ray, object):
+            ray = map(lambda p: PointWrapper(p), ray)
+            object = PointWrapper(object.centroid.point)
+
+            hand_to_object = object - ray[0]
+            pointing_direction = ray[1] - ray[0]
+
+            store_1 = np.cross(hand_to_object.to_numpy(), pointing_direction.to_numpy())
+
+            return np.linalg.norm(store_1) / np.linalg.norm(
+                pointing_direction.to_numpy()
+            )
+
+        def transform_object_to_map(object):
+            try:
+                trans = self.tf2_buffer.lookup_transform(
+                    "map", object.centroid.header.frame_id, object.centroid.header.stamp
+                )
+            except (LookupException | ExtrapolationException) as ex:
+                rospy.logwarn(ex)
+                return
+
+            object.centroid = do_transform_point(object.centroid, trans)
+            # object.mean_point = do_transform_point(object.mean_point, trans)
+            # object.bounding_box = map(
+            #     lambda p: do_transform_point(p, trans), object.bounding_box
+            # )
+            # object.eigen_vectors = map(
+            #     lambda v: do_transform_vector3(v, trans), object.eigen_vectors
+            # )
+
+            return object
+
+        if pointing_ray is None:
+            pointing_ray = GestureRecognitionInterface.get_pointing_ray()
+
+        objects = map(transform_object_to_map, objects)
+
+        return sorted(objects, key=lambda o: ray_distance(pointing_ray, o))