From f764ca38f87c657bcdb4777a2fda31b11683a7ad Mon Sep 17 00:00:00 2001 From: Ralf Bernitt Date: Sun, 24 Nov 2024 22:29:51 +0100 Subject: [PATCH 1/5] Added clustering of radar data --- code/perception/src/radar_node.py | 57 +++++++++++++++++++++++++------ 1 file changed, 46 insertions(+), 11 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 04a9d59b8..ca3cded6f 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -2,8 +2,10 @@ import rospy import ros_numpy import numpy as np +from std_msgs.msg import String from sensor_msgs.msg import PointCloud2 -from std_msgs.msg import Float32 +from sklearn.cluster import DBSCAN +import json class RadarNode: @@ -12,19 +14,19 @@ class RadarNode: """ def callback(self, data): - """Process radar Point2Cloud data and publish minimum velocity. + """Process radar Point2Cloud data and publish clustered data points. - Extracts velocity information from radar data - and publishes the minimum velocity as a - Float32 message + Extracts information from radar data + and publishes the clustered radar data + points as a String message Args: - data: Point2Cloud message containing radar data with velocity field + data: Point2Cloud message containing radar data """ - coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - msg = np.min(coordinates["Velocity"]) - self.dist_array_radar_publisher.publish(msg) + clustered_points = cluster_radar_data_from_pointcloud(data, 10) + clustered_points_json = json.dumps(clustered_points) + self.dist_array_radar_publisher.publish(clustered_points_json) def listener(self): """ @@ -35,8 +37,10 @@ def listener(self): # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Radar/velocity"), - Float32, + rospy.get_param( + "~image_distance_topic", "/paf/hero/Radar/dist_array_unsegmented" + ), + String, queue_size=10, ) @@ -49,6 +53,37 @@ def listener(self): rospy.spin() +def pointcloud2_to_array(pointcloud_msg): + cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) + distances = np.sqrt( + cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 + ) + return np.column_stack( + (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) + ) + + +def cluster_radar_data_from_pointcloud( + pointcloud_msg, max_distance, eps=1.0, min_samples=2 +): + data = pointcloud2_to_array(pointcloud_msg) + filtered_data = data[data[:, 3] < max_distance] + filtered_data = filtered_data[ + (filtered_data[:, 1] >= -1) + & (filtered_data[:, 1] <= 1) + & (filtered_data[:, 2] <= 1.3) + & (filtered_data[:, 2] >= -0.7) + ] + if len(filtered_data) == 0: + return {} + coordinates = filtered_data[:, :2] + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) + labels = clustering.labels_ + clustered_points = {label: list(labels).count(label) for label in set(labels)} + clustered_points = {int(label): count for label, count in clustered_points.items()} + return clustered_points + + if __name__ == "__main__": radar_node = RadarNode() radar_node.listener() From 0540464f288556164c1d64244dcee3a76012f8b1 Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Mon, 25 Nov 2024 10:21:57 +0100 Subject: [PATCH 2/5] Update radar_node.py New functions have been annotated with comments. --- code/perception/src/radar_node.py | 50 +++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index ca3cded6f..fdb54caf8 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -54,10 +54,24 @@ def listener(self): def pointcloud2_to_array(pointcloud_msg): + """ + Konvertiert eine ROS-PointCloud2-Nachricht in ein NumPy-Array und berechnet die euklidischen Entfernungen + jedes Punkts vom Ursprung. + + Parameter: + - pointcloud_msg: sensor_msgs/PointCloud2 + Die ROS-PointCloud2-Nachricht, die die 3D-Punkte enthält. + + Rückgabewert: + - np.ndarray + Ein 2D-Array, in dem jede Zeile einem Punkt in der Punktwolke entspricht: + [x, y, z, distance], wobei "distance" die Entfernung vom Ursprung ist. + """ cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) distances = np.sqrt( cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 ) + return np.column_stack( (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) ) @@ -66,20 +80,56 @@ def pointcloud2_to_array(pointcloud_msg): def cluster_radar_data_from_pointcloud( pointcloud_msg, max_distance, eps=1.0, min_samples=2 ): + """ + Filtert und gruppiert Punkte aus einer ROS-PointCloud2-Nachricht basierend auf DBSCAN-Clustering. + + Parameter: + - pointcloud_msg: sensor_msgs/PointCloud2 + Die ROS-PointCloud2-Nachricht mit den 3D-Punkten. + - max_distance: float + Maximale Entfernung, um Punkte zu berücksichtigen. Punkte außerhalb dieser Entfernung werden verworfen. + - eps: float, optional (default: 1.0) + Der maximale Abstand zwischen zwei Punkten, damit diese im selben Cluster sein können. + - min_samples: int, optional (default: 2) + Die minimale Anzahl von Punkten, die erforderlich sind, um einen Cluster zu bilden. + + Rückgabewert: + - dict + Ein Dictionary, in dem die Schlüssel die Cluster-Labels (int) sind und die Werte die Anzahl der Punkte + in jedem Cluster. Wenn keine Punkte vorhanden sind, wird ein leeres Dictionary zurückgegeben. + + Funktionalität: + - Die Funktion konvertiert die Punktwolke in ein Array mit Punktkoordinaten und berechneten Entfernungen. + - Punkte außerhalb eines spezifizierten Entfernungsbereichs und räumlichen Filtergrenzen werden verworfen. + - Anschließend wird DBSCAN-Clustering auf die 2D-Koordinaten (x, y) der Punkte angewendet. + - Die Cluster-Labels und ihre Häufigkeiten werden als Dictionary zurückgegeben. + """ data = pointcloud2_to_array(pointcloud_msg) + + # Filtere Punkte basierend auf der maximalen Entfernung filtered_data = data[data[:, 3] < max_distance] + + # Zusätzlicher Filter auf Y- und Z-Werte filtered_data = filtered_data[ (filtered_data[:, 1] >= -1) & (filtered_data[:, 1] <= 1) & (filtered_data[:, 2] <= 1.3) & (filtered_data[:, 2] >= -0.7) ] + + # Rückgabe eines leeren Dictionaries, wenn keine Punkte übrig sind if len(filtered_data) == 0: return {} + + # Extrahiere die 2D-Koordinaten (x, y) für das Clustering coordinates = filtered_data[:, :2] clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) labels = clustering.labels_ + + # Zählen der Punktanzahl in jedem Cluster clustered_points = {label: list(labels).count(label) for label in set(labels)} + + # Konvertiere die Labels in Integer und gebe das Dictionary zurück clustered_points = {int(label): count for label, count in clustered_points.items()} return clustered_points From 30a569ce1ec4ed97eb4c8a516eba6335ffb2d0fa Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Mon, 25 Nov 2024 10:33:15 +0100 Subject: [PATCH 3/5] Update radar_node.py Whitespace elimination. --- code/perception/src/radar_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index fdb54caf8..2ace7d92c 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -128,7 +128,7 @@ def cluster_radar_data_from_pointcloud( # Zählen der Punktanzahl in jedem Cluster clustered_points = {label: list(labels).count(label) for label in set(labels)} - + # Konvertiere die Labels in Integer und gebe das Dictionary zurück clustered_points = {int(label): count for label, count in clustered_points.items()} return clustered_points From b99d8f9872d101a5ffd9bccbbd7d2763211d6e0a Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Mon, 25 Nov 2024 10:38:03 +0100 Subject: [PATCH 4/5] Update radar_node.py maximal line length of 88 characters. --- code/perception/src/radar_node.py | 45 ++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 2ace7d92c..eb00bd5c9 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -18,7 +18,7 @@ def callback(self, data): Extracts information from radar data and publishes the clustered radar data - points as a String message + points as a String message. Args: data: Point2Cloud message containing radar data @@ -38,7 +38,8 @@ def listener(self): # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( rospy.get_param( - "~image_distance_topic", "/paf/hero/Radar/dist_array_unsegmented" + "~image_distance_topic", + "/paf/hero/Radar/dist_array_unsegmented", ), String, queue_size=10, @@ -55,8 +56,8 @@ def listener(self): def pointcloud2_to_array(pointcloud_msg): """ - Konvertiert eine ROS-PointCloud2-Nachricht in ein NumPy-Array und berechnet die euklidischen Entfernungen - jedes Punkts vom Ursprung. + Konvertiert eine ROS-PointCloud2-Nachricht in ein NumPy-Array und berechnet + die euklidischen Entfernungen jedes Punkts vom Ursprung. Parameter: - pointcloud_msg: sensor_msgs/PointCloud2 @@ -81,27 +82,35 @@ def cluster_radar_data_from_pointcloud( pointcloud_msg, max_distance, eps=1.0, min_samples=2 ): """ - Filtert und gruppiert Punkte aus einer ROS-PointCloud2-Nachricht basierend auf DBSCAN-Clustering. + Filtert und gruppiert Punkte aus einer ROS-PointCloud2-Nachricht + basierend auf DBSCAN-Clustering. Parameter: - pointcloud_msg: sensor_msgs/PointCloud2 Die ROS-PointCloud2-Nachricht mit den 3D-Punkten. - max_distance: float - Maximale Entfernung, um Punkte zu berücksichtigen. Punkte außerhalb dieser Entfernung werden verworfen. + Maximale Entfernung, um Punkte zu berücksichtigen. Punkte außerhalb dieser + Entfernung werden verworfen. - eps: float, optional (default: 1.0) - Der maximale Abstand zwischen zwei Punkten, damit diese im selben Cluster sein können. + Der maximale Abstand zwischen zwei Punkten, damit diese im selben Cluster + sein können. - min_samples: int, optional (default: 2) - Die minimale Anzahl von Punkten, die erforderlich sind, um einen Cluster zu bilden. + Die minimale Anzahl von Punkten, die erforderlich sind, um einen Cluster + zu bilden. Rückgabewert: - dict - Ein Dictionary, in dem die Schlüssel die Cluster-Labels (int) sind und die Werte die Anzahl der Punkte - in jedem Cluster. Wenn keine Punkte vorhanden sind, wird ein leeres Dictionary zurückgegeben. + Ein Dictionary, in dem die Schlüssel die Cluster-Labels (int) sind und die + Werte die Anzahl der Punkte in jedem Cluster. Wenn keine Punkte vorhanden + sind, wird ein leeres Dictionary zurückgegeben. Funktionalität: - - Die Funktion konvertiert die Punktwolke in ein Array mit Punktkoordinaten und berechneten Entfernungen. - - Punkte außerhalb eines spezifizierten Entfernungsbereichs und räumlichen Filtergrenzen werden verworfen. - - Anschließend wird DBSCAN-Clustering auf die 2D-Koordinaten (x, y) der Punkte angewendet. + - Die Funktion konvertiert die Punktwolke in ein Array mit Punktkoordinaten + und berechneten Entfernungen. + - Punkte außerhalb eines spezifizierten Entfernungsbereichs und räumlichen + Filtergrenzen werden verworfen. + - Anschließend wird DBSCAN-Clustering auf die 2D-Koordinaten (x, y) der Punkte + angewendet. - Die Cluster-Labels und ihre Häufigkeiten werden als Dictionary zurückgegeben. """ data = pointcloud2_to_array(pointcloud_msg) @@ -127,10 +136,16 @@ def cluster_radar_data_from_pointcloud( labels = clustering.labels_ # Zählen der Punktanzahl in jedem Cluster - clustered_points = {label: list(labels).count(label) for label in set(labels)} + clustered_points = { + label: list(labels).count(label) + for label in set(labels) + } # Konvertiere die Labels in Integer und gebe das Dictionary zurück - clustered_points = {int(label): count for label, count in clustered_points.items()} + clustered_points = { + int(label): count + for label, count in clustered_points.items() + } return clustered_points From b4e217cb2c16e23a47f7889891340c54ca986f0e Mon Sep 17 00:00:00 2001 From: michalal7 <53532256+michalal7@users.noreply.github.com> Date: Mon, 25 Nov 2024 10:48:15 +0100 Subject: [PATCH 5/5] Update radar_node.py Translated comments to englisch and reformated code. --- code/perception/src/radar_node.py | 90 +++++++++---------------------- 1 file changed, 25 insertions(+), 65 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index eb00bd5c9..4ddf5b4dd 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -9,9 +9,7 @@ class RadarNode: - """See doc/perception/radar_node.md on - how to configure this node - """ + """See doc/perception/radar_node.md on how to configure this node.""" def callback(self, data): """Process radar Point2Cloud data and publish clustered data points. @@ -23,56 +21,46 @@ def callback(self, data): Args: data: Point2Cloud message containing radar data """ - clustered_points = cluster_radar_data_from_pointcloud(data, 10) clustered_points_json = json.dumps(clustered_points) self.dist_array_radar_publisher.publish(clustered_points_json) def listener(self): - """ - Initializes the node and it's publishers - """ - # run simultaneously. + """Initializes the node and its publishers.""" rospy.init_node("radar_node") - - # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( rospy.get_param( - "~image_distance_topic", - "/paf/hero/Radar/dist_array_unsegmented", + "~image_distance_topic", "/paf/hero/Radar/dist_array_unsegmented" ), String, queue_size=10, ) - rospy.Subscriber( rospy.get_param("~source_topic", "/carla/hero/RADAR"), PointCloud2, self.callback, ) - rospy.spin() def pointcloud2_to_array(pointcloud_msg): """ - Konvertiert eine ROS-PointCloud2-Nachricht in ein NumPy-Array und berechnet - die euklidischen Entfernungen jedes Punkts vom Ursprung. + Converts a ROS PointCloud2 message into a NumPy array and calculates the + Euclidean distance of each point from the origin. - Parameter: + Parameters: - pointcloud_msg: sensor_msgs/PointCloud2 - Die ROS-PointCloud2-Nachricht, die die 3D-Punkte enthält. + The ROS PointCloud2 message containing the 3D points. - Rückgabewert: + Returns: - np.ndarray - Ein 2D-Array, in dem jede Zeile einem Punkt in der Punktwolke entspricht: - [x, y, z, distance], wobei "distance" die Entfernung vom Ursprung ist. + A 2D array where each row corresponds to a point in the point cloud: + [x, y, z, distance], where "distance" is the distance from the origin. """ cloud_array = ros_numpy.point_cloud2.pointcloud2_to_array(pointcloud_msg) distances = np.sqrt( cloud_array["x"] ** 2 + cloud_array["y"] ** 2 + cloud_array["z"] ** 2 ) - return np.column_stack( (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances) ) @@ -82,70 +70,42 @@ def cluster_radar_data_from_pointcloud( pointcloud_msg, max_distance, eps=1.0, min_samples=2 ): """ - Filtert und gruppiert Punkte aus einer ROS-PointCloud2-Nachricht - basierend auf DBSCAN-Clustering. + Filters and clusters points from a ROS PointCloud2 message based on DBSCAN + clustering. - Parameter: + Parameters: - pointcloud_msg: sensor_msgs/PointCloud2 - Die ROS-PointCloud2-Nachricht mit den 3D-Punkten. + The ROS PointCloud2 message containing the 3D points. - max_distance: float - Maximale Entfernung, um Punkte zu berücksichtigen. Punkte außerhalb dieser - Entfernung werden verworfen. + Maximum distance to consider points. Points beyond this distance are + discarded. - eps: float, optional (default: 1.0) - Der maximale Abstand zwischen zwei Punkten, damit diese im selben Cluster - sein können. + The maximum distance between two points for them to be considered in + the same cluster. - min_samples: int, optional (default: 2) - Die minimale Anzahl von Punkten, die erforderlich sind, um einen Cluster - zu bilden. + The minimum number of points required to form a cluster. - Rückgabewert: + Returns: - dict - Ein Dictionary, in dem die Schlüssel die Cluster-Labels (int) sind und die - Werte die Anzahl der Punkte in jedem Cluster. Wenn keine Punkte vorhanden - sind, wird ein leeres Dictionary zurückgegeben. - - Funktionalität: - - Die Funktion konvertiert die Punktwolke in ein Array mit Punktkoordinaten - und berechneten Entfernungen. - - Punkte außerhalb eines spezifizierten Entfernungsbereichs und räumlichen - Filtergrenzen werden verworfen. - - Anschließend wird DBSCAN-Clustering auf die 2D-Koordinaten (x, y) der Punkte - angewendet. - - Die Cluster-Labels und ihre Häufigkeiten werden als Dictionary zurückgegeben. + A dictionary where the keys are cluster labels (int) and the values + are the number of points in each cluster. Returns an empty dictionary + if no points are available. """ data = pointcloud2_to_array(pointcloud_msg) - - # Filtere Punkte basierend auf der maximalen Entfernung filtered_data = data[data[:, 3] < max_distance] - - # Zusätzlicher Filter auf Y- und Z-Werte filtered_data = filtered_data[ (filtered_data[:, 1] >= -1) & (filtered_data[:, 1] <= 1) & (filtered_data[:, 2] <= 1.3) & (filtered_data[:, 2] >= -0.7) ] - - # Rückgabe eines leeren Dictionaries, wenn keine Punkte übrig sind if len(filtered_data) == 0: return {} - - # Extrahiere die 2D-Koordinaten (x, y) für das Clustering coordinates = filtered_data[:, :2] clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates) labels = clustering.labels_ - - # Zählen der Punktanzahl in jedem Cluster - clustered_points = { - label: list(labels).count(label) - for label in set(labels) - } - - # Konvertiere die Labels in Integer und gebe das Dictionary zurück - clustered_points = { - int(label): count - for label, count in clustered_points.items() - } + clustered_points = {label: list(labels).count(label) for label in set(labels)} + clustered_points = {int(label): count for label, count in clustered_points.items()} return clustered_points