From 078c7dff2f8641232304a41ab33314477189e19a Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Wed, 13 Dec 2023 12:47:04 +0100 Subject: [PATCH 1/8] feat(144): min distance lidar --- code/agent/src/agent/agent.py | 2 +- code/perception/launch/perception.launch | 18 ++--- code/perception/msg/MinDistance.msg | 1 + code/perception/src/lidar_distance.py | 94 ++++++++++++++++++++++-- code/requirements.txt | 3 +- 5 files changed, 102 insertions(+), 16 deletions(-) create mode 100644 code/perception/msg/MinDistance.msg diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index e2732f00..d32f8907 100755 --- a/code/agent/src/agent/agent.py +++ b/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.0, '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, diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 3adc596e..eae08eb0 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -29,12 +29,12 @@ - + - + - + --> + @@ -68,13 +68,13 @@ - - + + - + diff --git a/code/perception/msg/MinDistance.msg b/code/perception/msg/MinDistance.msg new file mode 100644 index 00000000..a62513a7 --- /dev/null +++ b/code/perception/msg/MinDistance.msg @@ -0,0 +1 @@ +float32 distance \ No newline at end of file diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 71e94d31..9c07c23d 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,8 +3,17 @@ 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 perception.msg import MinDistance + +from sklearn.cluster import DBSCAN +from sklearn.preprocessing import StandardScaler +import matplotlib.pyplot as plt +# 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 class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on @@ -53,6 +62,13 @@ def callback(self, data): lidar_filter_utility.remove_field_name(coordinates, 'intensity') .tolist() ) + plot = self.plot_blob(coordinates_xyz) + img_msg = self.bridge.cv2_to_imgmsg(plot, + encoding="passthrough") + img_msg.header = data.header + self.publisher.publish(img_msg) + + """ distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -62,7 +78,7 @@ def callback(self, data): range_msg.min_range = min(distances) range_msg.range = min(distances) - self.pub_range.publish(range_msg) + self.pub_range.publish(range_msg)""" def listener(self): """ Initializes the node and it's publishers @@ -77,20 +93,88 @@ def listener(self): ), PointCloud2 ) + self.pub_range = rospy.Publisher( rospy.get_param( '~range_topic', - '/carla/hero/' + rospy.get_namespace() + '_range' + '/carla/hero/' + rospy.get_namespace() + '_min_distance' + ), + MinDistance + ) + + self.publisher = rospy.Publisher( + rospy.get_param( + '~image_distance_topic', + '/paf/hero/Center/segmented_image' ), - Range + ImageMsg ) + self.bridge = CvBridge() + 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): + # Standardize the data + xyz_coords_standardized = StandardScaler().fit_transform(xyz_coords) + + # Compute pairwise distances in the x-axis + pairwise_distances_x = np.abs(np.subtract.outer(xyz_coords[:, 0], + xyz_coords[:, 0])) + + # Find the absolute minimum x distance + min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0]) + + # print(f"Absolute minimum x + # distance: {min_x_distance:.4f} meters") + self.pub_range.publish(min_x_distance) + + # Apply DBSCAN algorithm + # Maximum distance between two samples for one to be + # considered as in the neighborhood of the other + eps = 0.2 + + # The number of samples in a neighborhood for a point + # to be considered as a core point + min_samples = 2 + dbscan = DBSCAN(eps=eps, min_samples=min_samples) + labels = dbscan.fit_predict(xyz_coords_standardized) + + # Plot the clustered points + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + for label in set(labels): + if label == -1: # Noise points + 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) + + # Set axis labels + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + + fig.canvas.draw() + + # convert canvas to image + img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8, sep='') + img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,)) + + # img is rgb, convert to opencv's default bgr + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + return img + if __name__ == '__main__': lidar_distance = LidarDistance() diff --git a/code/requirements.txt b/code/requirements.txt index 3b0948d0..8efeaa45 100644 --- a/code/requirements.txt +++ b/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 \ No newline at end of file +ultralytics==8.0.220 +scikit-learn>=0.18 \ No newline at end of file From 58249f6f7d486c9d46fd5b130922174f669e7399 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Wed, 13 Dec 2023 12:47:31 +0100 Subject: [PATCH 2/8] feat(144): min distance lidar --- code/perception/src/lidar_distance.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 9c07c23d..7a00b2cd 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -15,6 +15,7 @@ from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge + class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on how to configute this node @@ -129,16 +130,16 @@ def plot_blob(self, xyz_coords): # Find the absolute minimum x distance min_x_distance = np.min(pairwise_distances_x[pairwise_distances_x > 0]) - # print(f"Absolute minimum x + # print(f"Absolute minimum x # distance: {min_x_distance:.4f} meters") self.pub_range.publish(min_x_distance) # Apply DBSCAN algorithm - # Maximum distance between two samples for one to be + # Maximum distance between two samples for one to be # considered as in the neighborhood of the other eps = 0.2 - - # The number of samples in a neighborhood for a point + + # The number of samples in a neighborhood for a point # to be considered as a core point min_samples = 2 dbscan = DBSCAN(eps=eps, min_samples=min_samples) @@ -150,14 +151,14 @@ def plot_blob(self, xyz_coords): for label in set(labels): if label == -1: # Noise points - ax.scatter(xyz_coords[labels == label][:, 0], - xyz_coords[labels == label][:, 1], - xyz_coords[labels == label][:, 2], + 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], + ax.scatter(xyz_coords[labels == label][:, 0], + xyz_coords[labels == label][:, 1], + xyz_coords[labels == label][:, 2], label=f'Cluster {label + 1}', s=50) # Set axis labels From a45bd08e92df8639a2c91031d96b9f6ade84c288 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 14 Dec 2023 17:31:11 +0100 Subject: [PATCH 3/8] feat(145): distance to objects --- code/agent/src/agent/agent.py | 2 +- code/perception/launch/perception.launch | 12 ++++++------ code/perception/src/lidar_distance.py | 21 ++++++++++++++++++--- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index e2732f00..ad8d076c 100755 --- a/code/agent/src/agent/agent.py +++ b/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, diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 3adc596e..e9f9b272 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,11 +30,11 @@ - + + - + --> @@ -74,7 +74,7 @@ - + diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 71e94d31..9b20ad0a 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -4,7 +4,8 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2, Range - +from pylot_point_cloud import PointCloud +from numpy.linalg import inv class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on @@ -53,6 +54,21 @@ def callback(self, data): lidar_filter_utility.remove_field_name(coordinates, 'intensity') .tolist() ) + + # camera: width -> 300, height -> 200, fov -> 100 + im = np.identity(3) + im[0, 2] = (300 - 1) / 2.0 + im[1, 2] = (200 - 1) / 2.0 + im[0, 0] = im[1, 1] = (300 - 1) / (2.0 * np.tan(100 * np.pi / 360.0)) + ex = np.identity(3) + #distance = coordinates_xyz[0][0] + point = np.array([20, 20, 1]) + + c = np.matmul(point, inv(im)) + c = np.matmul(c, inv(ex)) + + print(c) + distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -87,11 +103,10 @@ def listener(self): 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() - if __name__ == '__main__': lidar_distance = LidarDistance() lidar_distance.listener() From b0e73880a619b656fda8f3da7034d0c9ce85f2a7 Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Thu, 14 Dec 2023 17:31:31 +0100 Subject: [PATCH 4/8] feat(145): distance of objects --- code/perception/src/lidar_distance.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 9b20ad0a..fe4e204f 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -4,9 +4,9 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2, Range -from pylot_point_cloud import PointCloud from numpy.linalg import inv + class LidarDistance(): """ See doc/06_perception/03_lidar_distance_utility.md on how to configute this node @@ -61,14 +61,14 @@ def callback(self, data): im[1, 2] = (200 - 1) / 2.0 im[0, 0] = im[1, 1] = (300 - 1) / (2.0 * np.tan(100 * np.pi / 360.0)) ex = np.identity(3) - #distance = coordinates_xyz[0][0] + # distance = coordinates_xyz[0][0] point = np.array([20, 20, 1]) - + c = np.matmul(point, inv(im)) c = np.matmul(c, inv(ex)) print(c) - + distances = np.array( [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) @@ -103,10 +103,11 @@ def listener(self): 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() + if __name__ == '__main__': lidar_distance = LidarDistance() lidar_distance.listener() From 64a90cc7d8c673a86aae83801a8801ccf95cbe9d Mon Sep 17 00:00:00 2001 From: Leon Okrusch Date: Fri, 15 Dec 2023 18:27:45 +0100 Subject: [PATCH 5/8] feat(145): distance to objects --- build/docker-compose.yml | 2 +- code/perception/launch/perception.launch | 6 ++-- code/perception/src/lidar_distance.py | 36 +++++++++++++++++++++--- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 1545f945..22176565 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,7 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound + command: /bin/bash CarlaUE4.sh -quality-level=Low -world-port=2000 -resx=800 -resy=600 -nosound # Use this image version to enable instance segmentation cameras: (it does not match the leaderboard version) # image: carlasim/carla:0.9.14 image: ghcr.io/una-auxme/paf23:leaderboard-2.0 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index e9f9b272..ea11f397 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -30,11 +30,11 @@ -