Merge pull request #146 from una-auxme/145-distance-of-objects

145 distance of objects

build/docker-compose.yml

@@ -57,7 +57,7 @@ services:
     tty: true
     shm_size: 2gb
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
-    # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+    #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
     command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
 
     logging:

code/agent/src/agent/agent.py

@@ -26,7 +26,7 @@ def sensors(self):
              'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
              'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100},
             {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR',
-             'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
+             'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
              'yaw': 0.0},
             {'type': 'sensor.other.radar', 'id': 'RADAR',
              'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,

code/perception/CMakeLists.txt

@@ -52,6 +52,7 @@ add_message_files(
   FILES
   Waypoint.msg
   LaneChange.msg
+  MinDistance.msg
 )
 
 ## Generate services in the 'srv' folder

code/perception/launch/perception.launch

@@ -29,13 +29,12 @@
     <param name="role_name" value="$(arg role_name)" />
   </node>
 
-
+    
   <node pkg="perception" type="vision_node.py" name="VisionNode" output="screen">
     <param name="role_name" value="$(arg role_name)" />
     <param name="side" value="Center" />
 
-    <!--
-      Object-Detection: 
+     <!-- Object-Detection: 
       - fasterrcnn_resnet50_fpn_v2 
       - fasterrcnn_mobilenet_v3_large_320_fpn
       - yolov8n
@@ -53,28 +52,28 @@
 
       Image-Segmentation:
       - deeplabv3_resnet101
-      - yolov8x-seg 
-    -->
-
+      - yolov8x-seg -->
+
     <param name="model" value="yolov8x-seg" />
   </node>
+
 
   <node pkg="perception" type="global_plan_distance_publisher.py" name="GlobalPlanDistance" output="screen">
     <param name="control_loop_rate" value="0.1" />
     <param name="role_name" value="$(arg role_name)" />
   </node>
 
-  <node pkg="perception" type="lidar_distance.py" name="lidar_distance" output="screen">
-    <param name="max_y" value="1.5"/>
-    <param name="min_y" value="-1.5"/>
-    <param name="min_x" value="3"/>
-    <param name="min_z" value="-1.5"/>
-    <param name="max_z" value="0"/>
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance" output="screen">
+    <param name="max_y" value="2.5"/>
+    <param name="min_y" value="-2.5"/>
+    <param name="min_x" value="2."/>
+    <param name="min_z" value="-1.3"/>
+    <param name="max_z" value="1."/>
     <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered"/>
     <param name="range_topic" value="/carla/hero/LIDAR_range"/>
   </node>
 
-  <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
+ <!-- <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
     <param name="min_y" value="-5"/>
     <param name="max_y" value="-2.5"/>
     <param name="max_x" value="0"/>
@@ -92,6 +91,6 @@
     <param name="max_z" value="0"/>
     <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
     <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
-  </node>
+  </node>-->
 
 </launch>

code/perception/msg/MinDistance.msg

@@ -0,0 +1 @@
+float32 distance

code/perception/src/lidar_distance.py

@@ -3,7 +3,18 @@
 import ros_numpy
 import numpy as np
 import lidar_filter_utility
-from sensor_msgs.msg import PointCloud2, Range
+from sensor_msgs.msg import PointCloud2
+
+from sklearn.cluster import DBSCAN
+from sklearn.preprocessing import StandardScaler
+import matplotlib.pyplot as plt
+from perception.msg import MinDistance
+# from mpl_toolkits.mplot3d import Axes3D
+# from itertools import combinations
+import cv2
+from sensor_msgs.msg import Image as ImageMsg
+from cv_bridge import CvBridge
+from matplotlib.colors import LinearSegmentedColormap
 
 
 class LidarDistance():
@@ -26,7 +37,7 @@ def callback(self, data):
 
         # https://stackoverflow.com/questions/44295375/how-to-slice-a-numpy-
         # ndarray-made-up-of-numpy-void-numbers
-        bit_mask = lidar_filter_utility.bounding_box(
+        min_dist_bit_mask = lidar_filter_utility.bounding_box(
             coordinates,
             max_x=rospy.get_param('~max_x', np.inf),
             min_x=rospy.get_param('~min_x', -np.inf),
@@ -36,39 +47,60 @@ def callback(self, data):
             min_z=rospy.get_param('~min_z', -np.inf),
         )
 
+        reconstruct_bit_mask = lidar_filter_utility.bounding_box(
+            coordinates,
+            max_x=rospy.get_param('~max_x', np.inf),
+            min_x=rospy.get_param('~min_x', -np.inf),
+            min_z=rospy.get_param('~min_z', -np.inf),
+        )
+
         # Filter coordinates based in generated bit_mask
-        coordinates = coordinates[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 \
-            .point_cloud2.array_to_pointcloud2(coordinates)
+            .point_cloud2.array_to_pointcloud2(min_dist_coordinates)
         coordinates_manipulated.header = data.header
 
         # Publish manipulated pointCloud2
         self.pub_pointcloud.publish(coordinates_manipulated)
 
         # https://stackoverflow.com/questions/1401712/how-can-the-euclidean-
         # distance-be-calculated-with-numpy
-        coordinates_xyz = np.array(
-            lidar_filter_utility.remove_field_name(coordinates, 'intensity')
+        min_dist_coordinates_xyz = np.array(
+            lidar_filter_utility.remove_field_name(min_dist_coordinates,
+                                                   'intensity')
             .tolist()
         )
-        distances = np.array(
-            [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz])
 
-        if len(distances) > 0:
-            range_msg = Range()
-            range_msg.max_range = max(distances)
-            range_msg.min_range = min(distances)
-            range_msg.range = min(distances)
+        reconstruct_coordinates_xyz = np.array(
+            lidar_filter_utility.remove_field_name(reconstruct_coordinates,
+                                                   'intensity')
+            .tolist()
+        )
 
-            self.pub_range.publish(range_msg)
+        # handle minimum distance
+        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)
+
+        # handle reconstruction of lidar points
+        rainbow_cloud = 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)
 
     def listener(self):
         """ Initializes the node and it's publishers
         """
         # run simultaneously.
         rospy.init_node('lidar_distance')
+        self.bridge = CvBridge()
 
         self.pub_pointcloud = rospy.Publisher(
             rospy.get_param(
@@ -77,20 +109,117 @@ def listener(self):
             ),
             PointCloud2
         )
-        self.pub_range = rospy.Publisher(
+
+        # publisher for the closest blob in the lidar point cloud
+        self.pub_min_dist = rospy.Publisher(
             rospy.get_param(
                 '~range_topic',
-                '/carla/hero/' + rospy.get_namespace() + '_range'
+                '/paf/hero/Center/min_distance'
             ),
-            Range
+            MinDistance
+        )
+
+        # publisher for reconstructed lidar image
+        self.rainbow_publisher = rospy.Publisher(
+            rospy.get_param(
+                '~image_distance_topic',
+                '/paf/hero/Center/rainbow_image'
+            ),
+            ImageMsg
+        )
+
+        # publisher for 3d blob graph
+        self.min_dist_img_publisher = rospy.Publisher(
+            rospy.get_param(
+                '~image_distance_topic',
+                '/paf/hero/Center/min_dist_image'
+            ),
+            ImageMsg
         )
 
         rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"),
                          PointCloud2, self.callback)
 
-        # spin() simply keeps python from exiting until this node is stopped
         rospy.spin()
 
+    def plot_blob(self, xyz_coords):
+        # creates a 3d graph thazt highlights blobs of points
+        xyz_coords_standardized = StandardScaler().fit_transform(xyz_coords)
+        pairwise_distances_x = np.abs(np.subtract.outer(xyz_coords[:, 0],
+                                                        xyz_coords[:, 0]))
+        min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0])
+        self.pub_min_dist.publish(min_x_distance)
+        eps = 0.2
+        min_samples = 5
+        dbscan = DBSCAN(eps=eps, min_samples=min_samples)
+        labels = dbscan.fit_predict(xyz_coords_standardized)
+
+        fig = plt.figure()
+        ax = fig.add_subplot(111, projection='3d')
+
+        for label in set(labels):
+            if label == -1:
+                ax.scatter(xyz_coords[labels == label][:, 0],
+                           xyz_coords[labels == label][:, 1],
+                           xyz_coords[labels == label][:, 2],
+                           c='gray', marker='o', label='Noise')
+            else:
+                ax.scatter(xyz_coords[labels == label][:, 0],
+                           xyz_coords[labels == label][:, 1],
+                           xyz_coords[labels == label][:, 2],
+                           label=f'Cluster {label + 1}', s=50)
+
+        ax.set_xlabel('X')
+        ax.set_ylabel('Y')
+        ax.set_zlabel('Z')
+
+        fig.canvas.draw()
+        img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8, sep='')
+        img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,))
+        img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+        return img
+
+    def reconstruct_img_from_lidar(self, coordinates_xyz):
+        # reconstruct 3d LIDAR-Data and calculate 2D Pixel
+        # according to Camera-World
+
+        # intrinsic matrix for camera:
+        # width -> 300, height -> 200, fov -> 100 (agent.py)
+        im = np.identity(3)
+        im[0, 2] = 300 / 2.0
+        im[1, 2] = 200 / 2.0
+        im[0, 0] = im[1, 1] = 300 / (2.0 * np.tan(100 * np.pi / 360.0))
+
+        # extrinsic matrix for camera
+        ex = np.zeros(shape=(3, 4))
+        ex[0][0] = 1
+        ex[1][1] = 1
+        ex[2][2] = 1
+        m = np.matmul(im, ex)
+
+        # reconstruct camera image with LIDAR-Data
+        img = np.zeros(shape=(200, 300), dtype=np.uint8)
+        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 <= 300 and y >= 0 and y <= 200:
+                img[199-y][299-x] = 255 - c[0]
+
+        # Rainbox color mapping to highlight distances
+        colors = [(0, 0, 0)] + [(1, 0, 0), (1, 1, 0),
+                                (0, 1, 0), (0, 1, 1),
+                                (0, 0, 1)]
+        cmap_name = 'rainbow'
+        rainbow_cmap = LinearSegmentedColormap.from_list(cmap_name,
+                                                         colors,
+                                                         N=256)
+
+        img_colored = (rainbow_cmap(img / np.max(img)) * 255).astype(np.uint8)
+        img_bgr = cv2.cvtColor(img_colored, cv2.COLOR_RGBA2BGR)
+
+        return img_bgr
+
 
 if __name__ == '__main__':
     lidar_distance = LidarDistance()

code/perception/src/vision_node.py

@@ -18,7 +18,6 @@
 from cv_bridge import CvBridge
 from torchvision.utils import draw_bounding_boxes, draw_segmentation_masks
 import numpy as np
-from time import perf_counter
 from ultralytics import NAS, YOLO, RTDETR, SAM, FastSAM
 """
 VisionNode:
@@ -129,22 +128,16 @@ def setup_camera_publishers(self):
         )
 
     def handle_camera_image(self, image):
-        startTime = perf_counter()
-
         # free up cuda memory
         if self.device == "cuda":
             torch.cuda.empty_cache()
 
-        print("Before Model: ", perf_counter() - startTime)
-
         if self.framework == "pyTorch":
             vision_result = self.predict_torch(image)
 
         if self.framework == "ultralytics":
             vision_result = self.predict_ultralytics(image)
 
-        print("After Model: ", perf_counter() - startTime)
-
         # publish image to rviz
         img_msg = self.bridge.cv2_to_imgmsg(vision_result,
                                             encoding="passthrough")
@@ -180,7 +173,6 @@ def predict_ultralytics(self, image):
         cv_image = self.bridge.imgmsg_to_cv2(img_msg=image,
                                              desired_encoding='passthrough')
         cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
-        print(cv_image.shape)
 
         output = self.model(cv_image)
 
@@ -193,7 +185,6 @@ def create_mask(self, input_image, model_output):
 
         output_predictions = output_predictions.to(dtype=torch.bool)
 
-        print(output_predictions.shape)
         transposed_image = np.transpose(input_image, (2, 0, 1))
         tensor_image = torch.tensor(transposed_image)
         tensor_image = tensor_image.to(dtype=torch.uint8)

code/requirements.txt

@@ -11,4 +11,5 @@ scipy==1.10.0
 xmltodict==0.13.0
 py-trees==2.1.6
 numpy==1.23.5
-ultralytics==8.0.220
+ultralytics==8.0.220
+scikit-learn>=0.18