feat(169): distance of objects publishes x,y,z

code/perception/src/lidar_distance.py

@@ -36,14 +36,14 @@ def callback(self, data):
 
         # https://stackoverflow.com/questions/44295375/how-to-slice-a-numpy-
         # ndarray-made-up-of-numpy-void-numbers
-        min_dist_bit_mask = lidar_filter_utility.bounding_box(
+        """min_dist_bit_mask = lidar_filter_utility.bounding_box(
             coordinates,
             max_x=50.,
             min_x=2.,
             min_z=rospy.get_param('~min_z', -np.inf),
             min_y=-2.5,
             max_y=2.5,
-        )
+        )"""
 
         reconstruct_bit_mask = lidar_filter_utility.bounding_box(
             coordinates,
@@ -53,24 +53,24 @@ def callback(self, data):
         )
 
         # Filter coordinates based in generated bit_mask
-        min_dist_coordinates = coordinates[min_dist_bit_mask]
+        # min_dist_coordinates = coordinates[min_dist_bit_mask]
         reconstruct_coordinates = coordinates[reconstruct_bit_mask]
 
         # Create pointcloud from manipulated data
-        coordinates_manipulated = ros_numpy \
+        """coordinates_manipulated = ros_numpy \
             .point_cloud2.array_to_pointcloud2(min_dist_coordinates)
-        coordinates_manipulated.header = data.header
+        coordinates_manipulated.header = data.header"""
 
         # Publish manipulated pointCloud2
-        self.pub_pointcloud.publish(coordinates_manipulated)
+        # self.pub_pointcloud.publish(coordinates_manipulated)
 
         # https://stackoverflow.com/questions/1401712/how-can-the-euclidean-
         # distance-be-calculated-with-numpy
-        min_dist_coordinates_xyz = np.array(
+        """min_dist_coordinates_xyz = np.array(
             lidar_filter_utility.remove_field_name(min_dist_coordinates,
                                                    'intensity')
             .tolist()
-        )
+        )"""
 
         reconstruct_coordinates_xyz = np.array(
             lidar_filter_utility.remove_field_name(reconstruct_coordinates,
@@ -79,23 +79,29 @@ def callback(self, data):
         )
 
         # handle minimum distance
-        if min_dist_coordinates_xyz.shape[0] > 0:
+        """if min_dist_coordinates_xyz.shape[0] > 0:
             plot = self.plot_blob(min_dist_coordinates_xyz)
             img_msg = self.bridge.cv2_to_imgmsg(plot,
                                                 encoding="passthrough")
             img_msg.header = data.header
             self.min_dist_img_publisher.publish(img_msg)
         else:
-            self.pub_min_dist.publish(np.inf)
+            self.pub_min_dist.publish(np.inf)"""
 
         # handle reconstruction of lidar points
-        rainbow_cloud = self.reconstruct_img_from_lidar(
+        dist_array = self.reconstruct_img_from_lidar(
             reconstruct_coordinates_xyz)
 
-        img_msg = self.bridge.cv2_to_imgmsg(rainbow_cloud,
-                                            encoding="passthrough")
-        img_msg.header = data.header
-        self.rainbow_publisher.publish(img_msg)
+        # img_msg = self.bridge.cv2_to_imgmsg(rainbow_cloud,
+        #                                    encoding="passthrough")
+        # img_msg.header = data.header
+        # self.rainbow_publisher.publish(img_msg)
+
+        dist_array_msg = \
+            self.bridge.cv2_to_imgmsg(dist_array,
+                                      encoding="passthrough")
+        dist_array_msg.header = data.header
+        self.dist_array_publisher.publish(dist_array_msg)
 
     def listener(self):
         """ Initializes the node and it's publishers
@@ -133,14 +139,24 @@ def listener(self):
             queue_size=10
         )
 
-        # publisher for 3d blob graph
-        self.min_dist_img_publisher = rospy.Publisher(
+        # publisher for 3d blob graph (Deprecated)
+        """self.min_dist_img_publisher = rospy.Publisher(
             rospy.get_param(
                 '~image_distance_topic',
                 '/paf/hero/Center/min_dist_image'
             ),
             ImageMsg,
             queue_size=10
+        )"""
+
+        # publisher for dist_array
+        self.dist_array_publisher = rospy.Publisher(
+            rospy.get_param(
+                '~image_distance_topic',
+                '/paf/hero/Center/dist_array'
+            ),
+            ImageMsg,
+            queue_size=10
         )
 
         rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"),
@@ -220,12 +236,15 @@ def reconstruct_img_from_lidar(self, coordinates_xyz):
 
         # reconstruct camera image with LIDAR-Data
         img = np.zeros(shape=(720, 1280), dtype=np.float32)
+        dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32)
         for c in coordinates_xyz:
             point = np.array([c[1], c[2], c[0], 1])
             pixel = np.matmul(m, point)
             x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2])
             if x >= 0 and x <= 1280 and y >= 0 and y <= 720:
                 img[719-y][1279-x] = c[0]
+                dist_array[719-y][1279-x] = \
+                    np.array([c[0], c[1], c[2]], dtype=np.float32)
 
         # Rainbox color mapping to highlight distances
         """colors = [(0, 0, 0)] + [(1, 0, 0), (1, 1, 0),
@@ -238,8 +257,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz):
 
         img_colored = (rainbow_cmap(img / np.max(img)) * 255).astype(np.uint8)
         img_bgr = cv2.cvtColor(img_colored, cv2.COLOR_RGBA2BGR)"""
-
-        return img
+        return dist_array
 
 
 if __name__ == '__main__':

code/perception/src/vision_node.py

@@ -83,10 +83,12 @@ def __init__(self, name, **kwargs):
         self.device = torch.device("cuda"
                                    if torch.cuda.is_available() else "cpu")
         self.depth_images = []
+        self.dist_arrays = []
 
         # publish / subscribe setup
         self.setup_camera_subscriptions()
-        self.setup_rainbow_subscription()
+        # self.setup_rainbow_subscription()
+        self.setup_dist_array_subscription()
         self.setup_camera_publishers()
         self.setup_object_distance_publishers()
         self.setup_traffic_light_publishers()
@@ -134,6 +136,14 @@ def setup_rainbow_subscription(self):
             qos_profile=1
         )
 
+    def setup_dist_array_subscription(self):
+        self.new_subscription(
+            msg_type=numpy_msg(ImageMsg),
+            callback=self.handle_dist_array,
+            topic='/paf/hero/Center/dist_array',
+            qos_profile=1
+        )
+
     def setup_camera_publishers(self):
         self.publisher = self.new_publisher(
             msg_type=numpy_msg(ImageMsg),
@@ -190,6 +200,25 @@ def handle_rainbow_image(self, image):
         else:
             self.logerr("Depth-Fiel build up! No distances available yet!")
 
+    def handle_dist_array(self, dist_array):
+        dist_array = \
+            self.bridge.imgmsg_to_cv2(img_msg=dist_array,
+                                      desired_encoding='passthrough')
+        print(dist_array.shape)
+        self.dist_arrays.append(dist_array)
+        # TODO: include buffer parameter into launch file
+        if len(self.depth_images) > 1:
+            self.dist_arrays.pop(0)
+            """if self.save:
+                for i in range(len(self.depth_images)):
+                    cv2.imshow(f"{i}", self.depth_images[i])
+
+                self.save = False
+                cv2.waitKey(0)  # Wait for any key press
+                cv2.destroyAllWindows()"""
+        else:
+            self.logerr("Depth-Fiel build up! No distances available yet!")
+
     def predict_torch(self, image):
         self.model.eval()
         cv_image = self.bridge.imgmsg_to_cv2(img_msg=image,
@@ -219,6 +248,8 @@ def predict_ultralytics(self, image):
         cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
 
         output = self.model(cv_image, half=True, verbose=False)
+
+        # handle distance of objects
         distance_output = []
         c_boxes = []
         c_labels = []
@@ -227,26 +258,41 @@ def predict_ultralytics(self, image):
             for box in boxes:
                 cls = box.cls.item()
                 pixels = box.xyxy[0]
-                if len(self.depth_images) > 0:
+                if len(self.dist_arrays) > 0:
                     distances = np.asarray(
-                        [self.depth_images[i][int(pixels[1]):int(pixels[3]):1,
-                                              int(pixels[0]):int(pixels[2]):1]
-                            for i in range(len(self.depth_images))])
-                    non_zero_filter = distances[distances != 0]
-
+                        [self.dist_arrays[i]
+                         [int(pixels[1]):int(pixels[3]):1,
+                          int(pixels[0]):int(pixels[2]):1, ::]
+                            for i in range(len(self.dist_arrays))])
+                    condition = distances[:, :, :, 0] != 0
+                    non_zero_filter = distances[condition]
                     if len(non_zero_filter) > 0:
-                        obj_dist = np.min(non_zero_filter)
+                        obj_dist_index = np.argmin(non_zero_filter[:, 0])
+                        obj_dist = non_zero_filter[obj_dist_index]
+                        abs_distance = np.sqrt(
+                            obj_dist[0]**2 +
+                            obj_dist[1]**2 +
+                            obj_dist[2]**2)
                     else:
-                        obj_dist = np.inf
+                        obj_dist = (np.inf, np.inf, np.inf)
+                        abs_distance = np.inf
 
                     c_boxes.append(torch.tensor(pixels))
-                    c_labels.append(f"Class: {cls}, Meters: {obj_dist}")
-                    distance_output.append([cls, obj_dist])
+                    c_labels.append(f"Class: {cls},"
+                                    f"Meters: {round(abs_distance, 2)},"
+                                    f"({round(obj_dist[0], 2)},"
+                                    f"{round(obj_dist[1], 2)},"
+                                    f"{round(obj_dist[2], 2)})")
+                    distance_output.append([cls,
+                                            abs_distance,
+                                            obj_dist[0],
+                                            obj_dist[1],
+                                            obj_dist[2]])
 
         # print(distance_output)
         # self.logerr(distance_output)
         self.distance_publisher.publish(
-            Float32MultiArray(data=distance_output))
+           Float32MultiArray(data=distance_output))
 
         transposed_image = np.transpose(cv_image, (2, 0, 1))
         image_np_with_detections = torch.tensor(transposed_image,
@@ -256,7 +302,7 @@ def predict_ultralytics(self, image):
             self.process_traffic_lights(output[0], cv_image, image.header)
 
         c_boxes = torch.stack(c_boxes)
-        print(image_np_with_detections.shape, c_boxes.shape, c_labels)
+        # print(image_np_with_detections.shape, c_boxes.shape, c_labels)
         box = draw_bounding_boxes(image_np_with_detections,
                                   c_boxes,
                                   c_labels,