145 distance of objects (#160)

* feat(145): distance of objects complete

* feat(145): visualization of distance

* Add files via upload

* Update 12_distance_to_objects.md

* Add files via upload

* Update 12_distance_to_objects.md

* Update 12_distance_to_objects.md

* feat(145): resolve merge conflict

code/agent/config/dev_objects.json

@@ -104,23 +104,23 @@
           "type": "sensor.lidar.ray_cast",
           "id": "LIDAR",
           "spawn_point": {
-            "x": 0.7,
-            "y": 0.4,
-            "z": 1.60,
+            "x": 0.0,
+            "y": 0.0,
+            "z": 1.70,
             "roll": 0.0,
             "pitch": 0.0,
             "yaw": 0.0
           },
-          "range": 85,
-          "rotation_frequency": 10,
+          "range": 50,
+          "rotation_frequency": 15,
           "channels": 64,
           "upper_fov": 10,
           "lower_fov": -30,
-          "points_per_second": 600000,
+          "points_per_second": 1000000,
           "atmosphere_attenuation_rate": 0.004,
           "dropoff_general_rate": 0.45,
           "dropoff_intensity_limit": 0.8,
-          "dropoff_zero_intensity": 0.4
+          "dropoff_zero_intensity": 0.2
         },
         {
           "type": "sensor.other.radar",

code/agent/src/agent/agent.py

@@ -23,10 +23,10 @@ def get_ros_entrypoint(self):
     def sensors(self):
         sensors = [
             {'type': 'sensor.camera.rgb', 'id': 'Center',
-             'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
-             'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100},
+             'x': 0.0, 'y': 0.0, 'z': 1.70, 'roll': 0.0, 'pitch': 0.0,
+             'yaw': 0.0, 'width': 1280, 'height': 720, 'fov': 100},
             {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR',
-             'x': 0.7, 'y': 0.0, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
+             'x': 0.0, 'y': 0.0, 'z': 1.70, '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/launch/perception.launch

@@ -60,6 +60,7 @@
       - deeplabv3_resnet101
       - yolov8x-seg 
     -->
+
     <param name="model" value="rtdetr-x" />
   </node>
 
@@ -77,8 +78,8 @@
  <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="min_x" value="1."/>
+    <param name="min_z" value="-1.6"/>
     <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"/>

code/perception/src/lidar_distance.py

@@ -4,17 +4,16 @@
 import numpy as np
 import lidar_filter_utility
 from sensor_msgs.msg import PointCloud2
-
+from std_msgs.msg import Float32
 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
+# from matplotlib.colors import LinearSegmentedColormap
 
 
 class LidarDistance():
@@ -39,12 +38,11 @@ def callback(self, data):
         # ndarray-made-up-of-numpy-void-numbers
         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),
-            max_y=rospy.get_param('~max_y', np.inf),
-            min_y=rospy.get_param('~min_y', -np.inf),
-            max_z=rospy.get_param('~max_z', np.inf),
+            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(
@@ -81,15 +79,19 @@ def callback(self, data):
         )
 
         # 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)
+        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)
 
         # 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
@@ -107,7 +109,8 @@ def listener(self):
                 '~point_cloud_topic',
                 '/carla/hero/' + rospy.get_namespace() + '_filtered'
             ),
-            PointCloud2
+            PointCloud2,
+            queue_size=10
         )
 
         # publisher for the closest blob in the lidar point cloud
@@ -116,7 +119,8 @@ def listener(self):
                 '~range_topic',
                 '/paf/hero/Center/min_distance'
             ),
-            MinDistance
+            Float32,
+            queue_size=10
         )
 
         # publisher for reconstructed lidar image
@@ -125,7 +129,8 @@ def listener(self):
                 '~image_distance_topic',
                 '/paf/hero/Center/rainbow_image'
             ),
-            ImageMsg
+            ImageMsg,
+            queue_size=10
         )
 
         # publisher for 3d blob graph
@@ -134,7 +139,8 @@ def listener(self):
                 '~image_distance_topic',
                 '/paf/hero/Center/min_dist_image'
             ),
-            ImageMsg
+            ImageMsg,
+            queue_size=10
         )
 
         rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"),
@@ -145,19 +151,34 @@ def listener(self):
     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)
+        """pairwise_distances_x = np.abs(np.subtract.outer(xyz_coords[:, 0],
+                                                        xyz_coords[:, 0]))"""
+
+        # publish minimum distance
         eps = 0.2
         min_samples = 5
         dbscan = DBSCAN(eps=eps, min_samples=min_samples)
         labels = dbscan.fit_predict(xyz_coords_standardized)
 
+        min_distance_cluster = []
+
+        # Iterate through each cluster
+        for label in set(labels):
+            if label != -1:  # Ignore noise points
+                cluster_points = xyz_coords[labels == label]
+                min_distance_cluster.append(np.min(cluster_points[:, 0]))
+
+        # Publish the minimum distance within clusters
+        if len(min_distance_cluster) > 0:
+            self.pub_min_dist.publish(min(min_distance_cluster))
+        else:
+            self.pub_min_dist.publish(np.inf)
+
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 
         for label in set(labels):
+            # print(label)
             if label == -1:
                 ax.scatter(xyz_coords[labels == label][:, 0],
                            xyz_coords[labels == label][:, 1],
@@ -186,9 +207,9 @@ def reconstruct_img_from_lidar(self, coordinates_xyz):
         # 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))
+        im[0, 2] = 1280 / 2.0
+        im[1, 2] = 720 / 2.0
+        im[0, 0] = im[1, 1] = 1280 / (2.0 * np.tan(100 * np.pi / 360.0))
 
         # extrinsic matrix for camera
         ex = np.zeros(shape=(3, 4))
@@ -198,16 +219,16 @@ def reconstruct_img_from_lidar(self, coordinates_xyz):
         m = np.matmul(im, ex)
 
         # reconstruct camera image with LIDAR-Data
-        img = np.zeros(shape=(200, 300), dtype=np.uint8)
+        img = np.zeros(shape=(720, 1280), 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 <= 300 and y >= 0 and y <= 200:
-                img[199-y][299-x] = 255 - c[0]
+            if x >= 0 and x <= 1280 and y >= 0 and y <= 720:
+                img[719-y][1279-x] = c[0]
 
         # Rainbox color mapping to highlight distances
-        colors = [(0, 0, 0)] + [(1, 0, 0), (1, 1, 0),
+        """colors = [(0, 0, 0)] + [(1, 0, 0), (1, 1, 0),
                                 (0, 1, 0), (0, 1, 1),
                                 (0, 0, 1)]
         cmap_name = 'rainbow'
@@ -216,9 +237,9 @@ def reconstruct_img_from_lidar(self, coordinates_xyz):
                                                          N=256)
 
         img_colored = (rainbow_cmap(img / np.max(img)) * 255).astype(np.uint8)
-        img_bgr = cv2.cvtColor(img_colored, cv2.COLOR_RGBA2BGR)
+        img_bgr = cv2.cvtColor(img_colored, cv2.COLOR_RGBA2BGR)"""
 
-        return img_bgr
+        return img
 
 
 if __name__ == '__main__':

code/perception/src/lidar_filter_utility.py

@@ -31,6 +31,10 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf,
 
     """
 
+    """print(min_x, max_x, "X")
+    print(min_y, max_y, "Y")
+    print(min_z, max_z, "Z")"""
+
     bound_x = np.logical_and(points['x'] > min_x, points['x'] < max_x)
     bound_y = np.logical_and(points['y'] > min_y, points['y'] < max_y)
     bound_z = np.logical_and(points['z'] > min_z, points['z'] < max_z)