Skip to content

Commit

Permalink
Update radar_node.py
Browse files Browse the repository at this point in the history
Translated comments to englisch and reformated code.
  • Loading branch information
michalal7 authored Nov 25, 2024
1 parent b99d8f9 commit b4e217c
Showing 1 changed file with 25 additions and 65 deletions.
90 changes: 25 additions & 65 deletions code/perception/src/radar_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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)
)
Expand All @@ -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


Expand Down

0 comments on commit b4e217c

Please sign in to comment.