From b29e3258838875981f98248d5cfbc9e71e406ab8 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 27 Nov 2024 16:17:12 +0100
Subject: [PATCH 01/10] added first draft of radar data visualization

---
 code/perception/src/radar_node.py | 115 +++++++++++++++++++++++++++++-
 1 file changed, 113 insertions(+), 2 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 4ddf5b4d..247a2df4 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -2,10 +2,12 @@
 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 String, Header
+from sensor_msgs.msg import PointCloud2, PointField
 from sklearn.cluster import DBSCAN
 import json
+from sensor_msgs import point_cloud2
+import struct
 
 
 class RadarNode:
@@ -25,6 +27,23 @@ def callback(self, data):
         clustered_points_json = json.dumps(clustered_points)
         self.dist_array_radar_publisher.publish(clustered_points_json)
 
+        # output array [x, y, z, distance]
+        dataarray = pointcloud2_to_array(data)
+
+        # input array [x, y, z, distance], max_distance, output: filtered data
+        # dataarray = filter_data(dataarray, 10)
+
+        # input array [x, y, z, distance], output: dict clustered
+        clustered_data = cluster_data(dataarray)
+
+        # input array [x, y, z, distance], clustered labels
+        cloud = create_pointcloud2(dataarray, clustered_data.labels_)
+
+        self.visualization_radar_publisher.publish(cloud)
+
+        cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray)
+        self.cluster_info_radar_publisher.publish(cluster_info)
+
     def listener(self):
         """Initializes the node and its publishers."""
         rospy.init_node("radar_node")
@@ -35,6 +54,20 @@ def listener(self):
             String,
             queue_size=10,
         )
+        self.visualization_radar_publisher = rospy.Publisher(
+            rospy.get_param(
+                "~image_distance_topic", "/paf/hero/Radar/Visualization"
+            ),
+            PointCloud2,
+            queue_size=10,
+        )
+        self.cluster_info_radar_publisher = rospy.Publisher(
+            rospy.get_param(
+                "~image_distance_topic", "/paf/hero/Radar/ClusterInfo"
+            ),
+            String,
+            queue_size=10,
+        )
         rospy.Subscriber(
             rospy.get_param("~source_topic", "/carla/hero/RADAR"),
             PointCloud2,
@@ -109,6 +142,84 @@ def cluster_radar_data_from_pointcloud(
     return clustered_points
 
 
+# filters radar data in distance, y, z direction
+def filter_data(data, max_distance):
+
+    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)
+    ]
+    return filtered_data
+
+
+# clusters data with DBSCAN
+def cluster_data(filtered_data, eps=0.2, min_samples=1):
+
+    if len(filtered_data) == 0:
+        return {}
+    coordinates = filtered_data[:, :2]
+    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates)
+    return clustering
+
+
+# generates random color for cluster
+def generate_color_map(num_clusters):
+    np.random.seed(42)
+    colors = np.random.randint(0, 255, size=(num_clusters, 3))
+    return colors
+
+
+# creates pointcloud2 for publishing clustered radar data
+def create_pointcloud2(clustered_points, cluster_labels):
+    header = Header()
+    header.stamp = rospy.Time.now()
+    header.frame_id = "hero/RADAR"
+
+    points = []
+    unique_labels = np.unique(cluster_labels)
+    colors = generate_color_map(len(unique_labels))
+
+    for i, point in enumerate(clustered_points):
+        x, y, z, _ = point
+        label = cluster_labels[i]
+
+        if label == -1:
+            r, g, b = 128, 128, 128
+        else:
+            r, g, b = colors[label]
+
+        rgb = struct.unpack('f', struct.pack('I', (r << 16) | (g << 8) | b))[0]
+        points.append([x, y, z, rgb])
+
+    fields = [
+        PointField('x', 0, PointField.FLOAT32, 1),
+        PointField('y', 4, PointField.FLOAT32, 1),
+        PointField('z', 8, PointField.FLOAT32, 1),
+        PointField('rgb', 12, PointField.FLOAT32, 1),
+    ]
+
+    return point_cloud2.create_cloud(header, fields, points)
+
+
+# generates string with label-id and cluster size
+def generate_cluster_labels_and_colors(clusters, data):
+    cluster_info = []
+
+    for label in set(clusters.labels_):
+        cluster_points = data[clusters.labels_ == label]
+        cluster_size = len(cluster_points)
+        if label != -1:
+            cluster_info.append({
+                "label": int(label),
+                "points_count": cluster_size
+            })
+
+    return json.dumps(cluster_info)
+
+
 if __name__ == "__main__":
     radar_node = RadarNode()
     radar_node.listener()

From 614869034793471e0a4c396da3eedce809cb7475 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Thu, 28 Nov 2024 16:15:49 +0100
Subject: [PATCH 02/10] Added velocity as clustering criteria

---
 code/perception/src/radar_node.py | 32 +++++++++++++++++++++----------
 1 file changed, 22 insertions(+), 10 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 247a2df4..08dede8e 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -5,6 +5,7 @@
 from std_msgs.msg import String, Header
 from sensor_msgs.msg import PointCloud2, PointField
 from sklearn.cluster import DBSCAN
+from sklearn.preprocessing import StandardScaler
 import json
 from sensor_msgs import point_cloud2
 import struct
@@ -23,9 +24,9 @@ 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)
+        # 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)
 
         # output array [x, y, z, distance]
         dataarray = pointcloud2_to_array(data)
@@ -91,11 +92,14 @@ def pointcloud2_to_array(pointcloud_msg):
         [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
-    )
+    # 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)
+    # )
     return np.column_stack(
-        (cloud_array["x"], cloud_array["y"], cloud_array["z"], distances)
+        (cloud_array["x"], cloud_array["y"], cloud_array["z"], cloud_array["Velocity"])
     )
 
 
@@ -160,8 +164,15 @@ def cluster_data(filtered_data, eps=0.2, min_samples=1):
 
     if len(filtered_data) == 0:
         return {}
-    coordinates = filtered_data[:, :2]
-    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates)
+    # coordinates = filtered_data[:, :2]
+    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates)
+
+    # worse than without scaling
+    # scaler = StandardScaler()
+    # data_scaled = scaler.fit_transform(filtered_data)
+    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled)
+
+    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data)
     return clustering
 
 
@@ -183,7 +194,8 @@ def create_pointcloud2(clustered_points, cluster_labels):
     colors = generate_color_map(len(unique_labels))
 
     for i, point in enumerate(clustered_points):
-        x, y, z, _ = point
+        # x, y, z, _ = point
+        x, y, z, v = point
         label = cluster_labels[i]
 
         if label == -1:

From f5bbe472694e29c32a5cc882cca3ac1b7c9f0c67 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 4 Dec 2024 16:17:28 +0100
Subject: [PATCH 03/10] started implementation of bounding boxes for detected
 radar clusters

---
 code/perception/src/radar_node.py | 176 ++++++++++++++++++++++++++++--
 1 file changed, 165 insertions(+), 11 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 08dede8e..39ed7ec0 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -8,6 +8,9 @@
 from sklearn.preprocessing import StandardScaler
 import json
 from sensor_msgs import point_cloud2
+from visualization_msgs.msg import Marker, MarkerArray
+from geometry_msgs.msg import Point
+
 import struct
 
 
@@ -37,14 +40,26 @@ def callback(self, data):
         # input array [x, y, z, distance], output: dict clustered
         clustered_data = cluster_data(dataarray)
 
+        # transformed_data = transform_data_to_2d(dataarray)
+
         # input array [x, y, z, distance], clustered labels
         cloud = create_pointcloud2(dataarray, clustered_data.labels_)
-
         self.visualization_radar_publisher.publish(cloud)
 
         cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray)
         self.cluster_info_radar_publisher.publish(cluster_info)
 
+        points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1)))
+        bounding_boxes = generate_bounding_boxes(points_with_labels)
+
+        marker_array = MarkerArray()
+        for label, bbox in bounding_boxes:
+            if label != -1:
+                marker = create_bounding_box_marker(label, bbox)
+                marker_array.markers.append(marker)
+
+        self.marker_visualization_radar_publisher.publish(marker_array)
+
     def listener(self):
         """Initializes the node and its publishers."""
         rospy.init_node("radar_node")
@@ -62,6 +77,13 @@ def listener(self):
             PointCloud2,
             queue_size=10,
         )
+        self.marker_visualization_radar_publisher = rospy.Publisher(
+            rospy.get_param(
+                "~image_distance_topic", "/paf/hero/Radar/Marker"
+            ),
+            MarkerArray,
+            queue_size=10,
+        )
         self.cluster_info_radar_publisher = rospy.Publisher(
             rospy.get_param(
                 "~image_distance_topic", "/paf/hero/Radar/ClusterInfo"
@@ -149,18 +171,20 @@ def cluster_radar_data_from_pointcloud(
 # filters radar data in distance, y, z direction
 def filter_data(data, max_distance):
 
-    filtered_data = data[data[:, 3] < max_distance]
+    # filtered_data = data[data[:, 3] < max_distance]
+    filtered_data = data
     filtered_data = filtered_data[
-        (filtered_data[:, 1] >= -1)
-        & (filtered_data[:, 1] <= 1)
-        & (filtered_data[:, 2] <= 1.3)
-        & (filtered_data[:, 2] >= -0.7)
+        # (filtered_data[:, 1] >= -1)
+        # & (filtered_data[:, 1] <= 1)
+        # & (filtered_data[:, 2] <= 1.3)
+        (filtered_data[:, 2] <= 1.3)
+        & (filtered_data[:, 2] >= -0.6)  # -0.7
     ]
     return filtered_data
 
 
 # clusters data with DBSCAN
-def cluster_data(filtered_data, eps=0.2, min_samples=1):
+def cluster_data(filtered_data, eps=0.8, min_samples=5):
 
     if len(filtered_data) == 0:
         return {}
@@ -168,11 +192,11 @@ def cluster_data(filtered_data, eps=0.2, min_samples=1):
     # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates)
 
     # worse than without scaling
-    # scaler = StandardScaler()
-    # data_scaled = scaler.fit_transform(filtered_data)
-    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled)
+    scaler = StandardScaler()
+    data_scaled = scaler.fit_transform(filtered_data)
+    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled)
 
-    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data)
+    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data)
     return clustering
 
 
@@ -216,6 +240,136 @@ def create_pointcloud2(clustered_points, cluster_labels):
     return point_cloud2.create_cloud(header, fields, points)
 
 
+def transform_data_to_2d(clustered_data):
+
+    transformed_points = clustered_data
+    transformed_points[:, 0] = clustered_data[:, 0]
+    transformed_points[:, 1] = clustered_data[:, 1]
+    transformed_points[:, 2] = 0
+    transformed_points[:, 3] = clustered_data[:, 3]
+
+    return transformed_points
+
+
+def calculate_aabb(cluster_points):
+    """_summary_
+
+    Args:
+        cluster_points (_type_): _description_
+
+    Returns:
+        _type_: _description_
+    """
+
+    # for 2d (top-down) boxes
+    # x_min = np.min(cluster_points[:, 0])
+    # x_max = np.max(cluster_points[:, 0])
+    # y_min = np.min(cluster_points[:, 1])
+    # y_max = np.max(cluster_points[:, 1])
+
+    # return x_min, x_max, y_min, y_max
+
+    # for 3d boxes
+    x_min = np.min(cluster_points[:, 0])
+    x_max = np.max(cluster_points[:, 0])
+    y_min = np.min(cluster_points[:, 1])
+    y_max = np.max(cluster_points[:, 1])
+    z_min = np.min(cluster_points[:, 2])
+    z_max = np.max(cluster_points[:, 2])
+    rospy.loginfo(f"Bounding box for label: X({x_min}, {x_max}), Y({y_min}, {y_max}), Z({z_min}, {z_max})")
+    return x_min, x_max, y_min, y_max, z_min, z_max
+
+
+def generate_bounding_boxes(points_with_labels):
+    """_summary_
+
+    Args:
+        points_with_labels (_type_): _description_
+
+    Returns:
+        _type_: _description_
+    """
+    bounding_boxes = []
+    unique_labels = np.unique(points_with_labels[:, -1])
+    for label in unique_labels:
+        if label == -1:
+            continue
+        cluster_points = points_with_labels[points_with_labels[:, -1] == label, :3]
+        bbox = calculate_aabb(cluster_points)
+        bounding_boxes.append((label, bbox))
+    return bounding_boxes
+
+
+def create_bounding_box_marker(label, bbox):
+    """_summary_
+
+    Returns:
+        _type_: _description_
+    """
+    # for 2d (top-down) boxes
+    # x_min, x_max, y_min, y_max = bbox
+
+    # for 3d boxes
+    x_min, x_max, y_min, y_max, z_min, z_max = bbox
+
+    marker = Marker()
+    marker.header.frame_id = "hero/RADAR"
+    marker.id = int(label)
+    # marker.type = Marker.LINE_STRIP
+    marker.type = Marker.LINE_LIST
+    marker.action = Marker.ADD
+    marker.scale.x = 0.1
+    marker.color.r = 1.0
+    marker.color.g = 1.0
+    marker.color.b = 0.0
+    marker.color.a = 1.0
+
+    # for 2d (top-down) boxes
+    # points = [
+    #     Point(x_min, y_min, 0),
+    #     Point(x_max, y_min, 0),
+    #     Point(x_max, y_max, 0),
+    #     Point(x_min, y_max, 0),
+    #     Point(x_min, y_min, 0),
+    # ]
+    # marker.points = points
+
+    # for 3d boxes
+    points = [
+        Point(x_min, y_min, z_min),  # Ecke 0
+        Point(x_max, y_min, z_min),  # Ecke 1
+        Point(x_max, y_max, z_min),  # Ecke 2
+        Point(x_min, y_max, z_min),  # Ecke 3
+        Point(x_min, y_min, z_max),  # Ecke 4
+        Point(x_max, y_min, z_max),  # Ecke 5
+        Point(x_max, y_max, z_max),  # Ecke 6
+        Point(x_min, y_max, z_max),  # Ecke 7
+
+        # Point(x_min, y_min, z_min),  # Verbinde z_min zu z_max
+        # Point(x_min, y_min, z_max),
+
+        # Point(x_max, y_min, z_min),
+        # Point(x_max, y_min, z_max),
+
+        # Point(x_max, y_max, z_min),
+        # Point(x_max, y_max, z_max),
+
+        # Point(x_min, y_max, z_min),
+        # Point(x_min, y_max, z_max),
+    ]
+    # marker.points = points
+    lines = [
+        (0, 1), (1, 2), (2, 3), (3, 0),  # Boden
+        (4, 5), (5, 6), (6, 7), (7, 4),  # Deckel
+        (0, 4), (1, 5), (2, 6), (3, 7),  # Vertikale Kanten
+    ]
+    for start, end in lines:
+        marker.points.append(points[start])
+        marker.points.append(points[end])
+    
+    return marker
+
+
 # generates string with label-id and cluster size
 def generate_cluster_labels_and_colors(clusters, data):
     cluster_info = []

From 07794547ed31d4af0743270c7c1480c8d0a49642 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Mon, 9 Dec 2024 17:17:27 +0100
Subject: [PATCH 04/10] added deletion of old markers

---
 code/perception/src/radar_node.py | 93 +++++++++++++++++++++++++++++--
 code/requirements.txt             |  3 +-
 2 files changed, 89 insertions(+), 7 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 39ed7ec0..ddff6511 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -46,20 +46,26 @@ def callback(self, data):
         cloud = create_pointcloud2(dataarray, clustered_data.labels_)
         self.visualization_radar_publisher.publish(cloud)
 
-        cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray)
-        self.cluster_info_radar_publisher.publish(cluster_info)
-
         points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1)))
         bounding_boxes = generate_bounding_boxes(points_with_labels)
 
         marker_array = MarkerArray()
+        # marker_array = clear_old_markers(marker_array)
         for label, bbox in bounding_boxes:
             if label != -1:
                 marker = create_bounding_box_marker(label, bbox)
                 marker_array.markers.append(marker)
+                # min_marker, max_marker = create_min_max_markers(label, bbox)
+                # marker_array.markers.append(min_marker)
+                # marker_array.markers.append(max_marker)
+
+        marker_array = clear_old_markers(marker_array, max_id=len(bounding_boxes) - 1)
 
         self.marker_visualization_radar_publisher.publish(marker_array)
 
+        cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray, marker_array, bounding_boxes)
+        self.cluster_info_radar_publisher.publish(cluster_info)
+
     def listener(self):
         """Initializes the node and its publishers."""
         rospy.init_node("radar_node")
@@ -366,12 +372,85 @@ def create_bounding_box_marker(label, bbox):
     for start, end in lines:
         marker.points.append(points[start])
         marker.points.append(points[end])
-    
+
     return marker
 
 
+def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1.0, 0.0, 1.0), max_color=(1.0, 0.0, 0.0, 1.0)):
+    """
+    Erstellt RViz-Marker für die Min- und Max-Punkte einer Bounding Box.
+
+    Args:
+        label (int): Die ID des Clusters (wird als Marker-ID genutzt).
+        bbox (tuple): Min- und Max-Werte der Bounding Box (x_min, x_max, y_min, y_max, z_min, z_max).
+        frame_id (str): Frame ID, in dem die Marker gezeichnet werden.
+        min_color (tuple): RGBA-Farbwerte für den Min-Punkt-Marker.
+        max_color (tuple): RGBA-Farbwerte für den Max-Punkt-Marker.
+
+    Returns:
+        tuple: Ein Paar von Markern (min_marker, max_marker).
+    """
+    x_min, x_max, y_min, y_max, z_min, z_max = bbox
+
+    # marker = Marker()
+    # marker.header.frame_id = "hero/RADAR"
+    # marker.id = int(label)
+    # # marker.type = Marker.LINE_STRIP
+    # marker.type = Marker.LINE_LIST
+    # marker.action = Marker.ADD
+    # marker.scale.x = 0.1
+    # marker.color.r = 1.0
+    # marker.color.g = 1.0
+    # marker.color.b = 0.0
+    # marker.color.a = 1.0
+
+    # Min-Punkt-Marker
+    min_marker = Marker()
+    min_marker.header.frame_id = frame_id
+    min_marker.id = int(label * 10)  # ID für Min-Punkt
+    min_marker.type = Marker.SPHERE
+    min_marker.action = Marker.ADD
+    min_marker.scale.x = 0.2  # Größe des Punktes
+    min_marker.scale.y = 0.2
+    min_marker.scale.z = 0.2
+    min_marker.color.r = min_color[0]
+    min_marker.color.g = min_color[1]
+    min_marker.color.b = min_color[2]
+    min_marker.color.a = min_color[3]
+    min_marker.pose.position.x = x_min
+    min_marker.pose.position.y = y_min
+    min_marker.pose.position.z = z_min
+
+    # Max-Punkt-Marker
+    max_marker = Marker()
+    max_marker.header.frame_id = frame_id
+    max_marker.id = int(label * 10 + 1)  # ID für Max-Punkt
+    max_marker.type = Marker.SPHERE
+    max_marker.action = Marker.ADD
+    max_marker.scale.x = 0.2
+    max_marker.scale.y = 0.2
+    max_marker.scale.z = 0.2
+    max_marker.color.r = max_color[0]
+    max_marker.color.g = max_color[1]
+    max_marker.color.b = max_color[2]
+    max_marker.color.a = max_color[3]
+    max_marker.pose.position.x = x_max
+    max_marker.pose.position.y = y_max
+    max_marker.pose.position.z = z_max
+
+    return min_marker, max_marker
+
+
+def clear_old_markers(marker_array, max_id):
+    """Löscht alte Marker aus dem MarkerArray."""
+    for marker in marker_array.markers:
+        if marker.id >= max_id:
+            marker.action = Marker.DELETE
+    return marker_array
+
+
 # generates string with label-id and cluster size
-def generate_cluster_labels_and_colors(clusters, data):
+def generate_cluster_labels_and_colors(clusters, data, marker_array, bounding_boxes):
     cluster_info = []
 
     for label in set(clusters.labels_):
@@ -380,7 +459,9 @@ def generate_cluster_labels_and_colors(clusters, data):
         if label != -1:
             cluster_info.append({
                 "label": int(label),
-                "points_count": cluster_size
+                "points_count": cluster_size,
+                "Anzahl marker": len(marker_array.markers),
+                "Anzahl Boundingboxen": len(bounding_boxes)
             })
 
     return json.dumps(cluster_info)
diff --git a/code/requirements.txt b/code/requirements.txt
index 55de87f0..cbe57476 100644
--- a/code/requirements.txt
+++ b/code/requirements.txt
@@ -16,4 +16,5 @@ numpy==1.23.5
 ultralytics==8.1.11
 scikit-learn>=0.18
 pandas==2.0.3
-debugpy==1.8.7
\ No newline at end of file
+debugpy==1.8.7
+pyopenssl==24.3.0
\ No newline at end of file

From 279944884dd72fe2a21180768d402da77bdce794 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 11 Dec 2024 15:12:52 +0100
Subject: [PATCH 05/10] added code documentation

---
 code/perception/src/radar_node.py | 315 +++++++++++++++++-------------
 1 file changed, 174 insertions(+), 141 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index ddff6511..975af2b1 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -27,34 +27,29 @@ 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)
 
-        # output array [x, y, z, distance]
         dataarray = pointcloud2_to_array(data)
 
-        # input array [x, y, z, distance], max_distance, output: filtered data
-        # dataarray = filter_data(dataarray, 10)
+        # radar position z=0.7
+        dataarray = filter_data(dataarray, min_z=-0.6)
 
-        # input array [x, y, z, distance], output: dict clustered
         clustered_data = cluster_data(dataarray)
 
         # transformed_data = transform_data_to_2d(dataarray)
 
-        # input array [x, y, z, distance], clustered labels
         cloud = create_pointcloud2(dataarray, clustered_data.labels_)
         self.visualization_radar_publisher.publish(cloud)
 
-        points_with_labels = np.hstack((dataarray, clustered_data.labels_.reshape(-1, 1)))
+        points_with_labels = np.hstack((dataarray,
+                                        clustered_data.labels_.reshape(-1, 1)))
         bounding_boxes = generate_bounding_boxes(points_with_labels)
 
         marker_array = MarkerArray()
-        # marker_array = clear_old_markers(marker_array)
         for label, bbox in bounding_boxes:
             if label != -1:
                 marker = create_bounding_box_marker(label, bbox)
                 marker_array.markers.append(marker)
+                # can be used for extra debugging
                 # min_marker, max_marker = create_min_max_markers(label, bbox)
                 # marker_array.markers.append(min_marker)
                 # marker_array.markers.append(max_marker)
@@ -63,7 +58,8 @@ def callback(self, data):
 
         self.marker_visualization_radar_publisher.publish(marker_array)
 
-        cluster_info = generate_cluster_labels_and_colors(clustered_data, dataarray, marker_array, bounding_boxes)
+        cluster_info = generate_cluster_info(clustered_data, dataarray, marker_array,
+                                             bounding_boxes)
         self.cluster_info_radar_publisher.publish(cluster_info)
 
     def listener(self):
@@ -117,93 +113,73 @@ def pointcloud2_to_array(pointcloud_msg):
     Returns:
     - np.ndarray
         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.
+        [x, y, z, Velocity]
     """
     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)
-    # )
     return np.column_stack(
         (cloud_array["x"], cloud_array["y"], cloud_array["z"], cloud_array["Velocity"])
     )
 
 
-def cluster_radar_data_from_pointcloud(
-    pointcloud_msg, max_distance, eps=1.0, min_samples=2
-):
+def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, max_z=100,
+                max_distance=100):
     """
-    Filters and clusters points from a ROS PointCloud2 message based on DBSCAN
-    clustering.
+    Filters radar data based on specified spatial and distance constraints.
 
-    Parameters:
-    - pointcloud_msg: sensor_msgs/PointCloud2
-        The ROS PointCloud2 message containing the 3D points.
-    - max_distance: float
-        Maximum distance to consider points. Points beyond this distance are
-        discarded.
-    - eps: float, optional (default: 1.0)
-        The maximum distance between two points for them to be considered in
-        the same cluster.
-    - min_samples: int, optional (default: 2)
-        The minimum number of points required to form a cluster.
+    This function applies multiple filtering criteria to the input radar data.
+    Points outside these bounds are excluded from the output.
+
+    Args:
+        data (np.ndarray): A 2D numpy array containing radar data, where each row 
+        represents a data point with the format [x, y, z, distance]. The array 
+        shape is (N, 4), where N is the number of points.
+        min_x (float, optional): Minimum value for the x-coordinate. Default is -1.
+        max_x (float, optional): Maximum value for the x-coordinate. Default is 1.
+        min_y (float, optional): Minimum value for the y-coordinate. Default is 1.
+        max_y (float, optional): Maximum value for the y-coordinate. Default is 1.
+        min_z (float, optional): Minimum value for the z-coordinate. Default is -0.7.
+        max_z (float, optional): Maximum value for the z-coordinate. Default is 1.3.
+        max_distance (float, optional): Maximum allowable distance of the point from
+        the sensor. Default is 100.
 
     Returns:
-    - dict
-        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.
+        np.ndarray: A numpy array containing only the filtered data points that meet 
+        the specified criteria.
     """
-    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)
+        (filtered_data[:, 0] >= min_x)
+        & (filtered_data[:, 0] <= max_x)
+        & (filtered_data[:, 1] >= min_y)
+        & (filtered_data[:, 1] <= max_y)
+        & (filtered_data[:, 2] <= max_z)
+        & (filtered_data[:, 2] >= min_z)
     ]
-    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
-
+    return filtered_data
 
-# filters radar data in distance, y, z direction
-def filter_data(data, max_distance):
 
-    # filtered_data = data[data[:, 3] < max_distance]
-    filtered_data = data
-    filtered_data = filtered_data[
-        # (filtered_data[:, 1] >= -1)
-        # & (filtered_data[:, 1] <= 1)
-        # & (filtered_data[:, 2] <= 1.3)
-        (filtered_data[:, 2] <= 1.3)
-        & (filtered_data[:, 2] >= -0.6)  # -0.7
-    ]
-    return filtered_data
+def cluster_data(data, eps=0.8, min_samples=3):
+    """_summary_
 
+    Args:
+        data (np.ndarray): data array which should be clustered
+        eps (float, optional): maximum distance of points. Defaults to 0.8.
+        min_samples (int, optional): min samples for 1 cluster. Defaults to 3.
 
-# clusters data with DBSCAN
-def cluster_data(filtered_data, eps=0.8, min_samples=5):
+    Returns:
+        dict: 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.
+    """
 
-    if len(filtered_data) == 0:
+    if len(data) == 0:
         return {}
-    # coordinates = filtered_data[:, :2]
-    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(coordinates)
-
-    # worse than without scaling
     scaler = StandardScaler()
-    data_scaled = scaler.fit_transform(filtered_data)
-    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled)
+    data_scaled = scaler.fit_transform(data)
+    clustered_points = DBSCAN(eps=eps, min_samples=min_samples).fit(data_scaled)
 
-    # clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(filtered_data)
-    return clustering
+    return clustered_points
 
 
 # generates random color for cluster
@@ -213,8 +189,16 @@ def generate_color_map(num_clusters):
     return colors
 
 
-# creates pointcloud2 for publishing clustered radar data
 def create_pointcloud2(clustered_points, cluster_labels):
+    """_summary_
+
+    Args:
+        clustered_points (dict): clustered points after dbscan
+        cluster_labels (_type_): _description_
+
+    Returns:
+        PointCloud2: pointcloud which can be published
+    """
     header = Header()
     header.stamp = rospy.Time.now()
     header.frame_id = "hero/RADAR"
@@ -224,7 +208,6 @@ def create_pointcloud2(clustered_points, cluster_labels):
     colors = generate_color_map(len(unique_labels))
 
     for i, point in enumerate(clustered_points):
-        # x, y, z, _ = point
         x, y, z, v = point
         label = cluster_labels[i]
 
@@ -247,6 +230,14 @@ def create_pointcloud2(clustered_points, cluster_labels):
 
 
 def transform_data_to_2d(clustered_data):
+    """_summary_
+
+    Args:
+        clustered_data (np.ndarray): clustered 3d data points
+
+    Returns:
+        _np.ndarray: clustered points, every z value is set to 0
+    """
 
     transformed_points = clustered_data
     transformed_points[:, 0] = clustered_data[:, 0]
@@ -258,13 +249,23 @@ def transform_data_to_2d(clustered_data):
 
 
 def calculate_aabb(cluster_points):
-    """_summary_
+    """
+    Calculates the axis-aligned bounding box (AABB) for a set of 3D points.
+
+    This function computes the minimum and maximum values along each axis (x, y, z)
+    for a given set of 3D points, which defines the bounding box that contains
+    all points in the cluster.
 
     Args:
-        cluster_points (_type_): _description_
+        cluster_points (numpy.ndarray): 
+        A 2D array where each row represents a 3D point (x, y, z).
+        The array should have shape (N, 3) where N is the number of points.
 
     Returns:
-        _type_: _description_
+        tuple: A tuple of the form (x_min, x_max, y_min, y_max, z_min, z_max),
+        which represents the axis-aligned bounding box (AABB) for the given
+        set of points. The values are the minimum and maximum coordinates
+        along the x, y, and z axes.
     """
 
     # for 2d (top-down) boxes
@@ -272,7 +273,7 @@ def calculate_aabb(cluster_points):
     # x_max = np.max(cluster_points[:, 0])
     # y_min = np.min(cluster_points[:, 1])
     # y_max = np.max(cluster_points[:, 1])
-
+    # rospy.loginfo(f"Bounding box: X({x_min}, {x_max}), Y({y_min}, {y_max})")
     # return x_min, x_max, y_min, y_max
 
     # for 3d boxes
@@ -282,18 +283,27 @@ def calculate_aabb(cluster_points):
     y_max = np.max(cluster_points[:, 1])
     z_min = np.min(cluster_points[:, 2])
     z_max = np.max(cluster_points[:, 2])
-    rospy.loginfo(f"Bounding box for label: X({x_min}, {x_max}), Y({y_min}, {y_max}), Z({z_min}, {z_max})")
     return x_min, x_max, y_min, y_max, z_min, z_max
 
 
 def generate_bounding_boxes(points_with_labels):
-    """_summary_
+    """
+    Generates bounding boxes for clustered points.
+
+    This function processes a set of points, each associated with a cluster label,
+    and generates an axis-aligned bounding box (AABB) for each unique cluster label.
 
     Args:
-        points_with_labels (_type_): _description_
+        points_with_labels (numpy.ndarray):
+        A 2D array of shape (N, 4) where each row contains
+        the coordinates (x, y, z) of a point along with its
+        corresponding cluster label in the last column.
+        The array should have the structure [x, y, z, label].
 
     Returns:
-        _type_: _description_
+        list: A list of tuples, where each tuple contains a cluster label and the
+              corresponding bounding box (bbox). The bbox is represented by a tuple
+              of the form (x_min, x_max, y_min, y_max, z_min, z_max).
     """
     bounding_boxes = []
     unique_labels = np.unique(points_with_labels[:, -1])
@@ -307,10 +317,23 @@ def generate_bounding_boxes(points_with_labels):
 
 
 def create_bounding_box_marker(label, bbox):
-    """_summary_
+    """
+    Creates an RViz Marker for visualizing a 3D bounding box.
+
+    This function generates a Marker object for RViz to visualize a 3D bounding box
+    based on the provided label and bounding box dimensions. The marker is
+    represented as a series of lines connecting the corners of the box.
+
+    Args:
+        label (int): The unique identifier for the cluster or object to which the
+        bounding box belongs. This label is used as the Marker ID.
+        bbox (tuple): A tuple containing the min and max coordinates of the bounding box
+                      in the format (x_min, x_max, y_min, y_max, z_min, z_max).
 
     Returns:
-        _type_: _description_
+        Marker: A Marker object that can be published to RViz to display the
+        3D bounding box. The marker is of type LINE_LIST,
+        representing the edges of the bounding box.
     """
     # for 2d (top-down) boxes
     # x_min, x_max, y_min, y_max = bbox
@@ -321,8 +344,8 @@ def create_bounding_box_marker(label, bbox):
     marker = Marker()
     marker.header.frame_id = "hero/RADAR"
     marker.id = int(label)
-    # marker.type = Marker.LINE_STRIP
-    marker.type = Marker.LINE_LIST
+    # marker.type = Marker.LINE_STRIP  # 2d boxes
+    marker.type = Marker.LINE_LIST  # 3d boxes
     marker.action = Marker.ADD
     marker.scale.x = 0.1
     marker.color.r = 1.0
@@ -342,32 +365,20 @@ def create_bounding_box_marker(label, bbox):
 
     # for 3d boxes
     points = [
-        Point(x_min, y_min, z_min),  # Ecke 0
-        Point(x_max, y_min, z_min),  # Ecke 1
-        Point(x_max, y_max, z_min),  # Ecke 2
-        Point(x_min, y_max, z_min),  # Ecke 3
-        Point(x_min, y_min, z_max),  # Ecke 4
-        Point(x_max, y_min, z_max),  # Ecke 5
-        Point(x_max, y_max, z_max),  # Ecke 6
-        Point(x_min, y_max, z_max),  # Ecke 7
-
-        # Point(x_min, y_min, z_min),  # Verbinde z_min zu z_max
-        # Point(x_min, y_min, z_max),
-
-        # Point(x_max, y_min, z_min),
-        # Point(x_max, y_min, z_max),
-
-        # Point(x_max, y_max, z_min),
-        # Point(x_max, y_max, z_max),
-
-        # Point(x_min, y_max, z_min),
-        # Point(x_min, y_max, z_max),
+        Point(x_min, y_min, z_min),
+        Point(x_max, y_min, z_min),
+        Point(x_max, y_max, z_min),
+        Point(x_min, y_max, z_min),
+        Point(x_min, y_min, z_max),
+        Point(x_max, y_min, z_max),
+        Point(x_max, y_max, z_max),
+        Point(x_min, y_max, z_max),
     ]
-    # marker.points = points
+
     lines = [
-        (0, 1), (1, 2), (2, 3), (3, 0),  # Boden
-        (4, 5), (5, 6), (6, 7), (7, 4),  # Deckel
-        (0, 4), (1, 5), (2, 6), (3, 7),  # Vertikale Kanten
+        (0, 1), (1, 2), (2, 3), (3, 0),  # Bottom
+        (4, 5), (5, 6), (6, 7), (7, 4),  # Top
+        (0, 4), (1, 5), (2, 6), (3, 7),  # Vertical Edges
     ]
     for start, end in lines:
         marker.points.append(points[start])
@@ -376,41 +387,33 @@ def create_bounding_box_marker(label, bbox):
     return marker
 
 
-def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1.0, 0.0, 1.0), max_color=(1.0, 0.0, 0.0, 1.0)):
+# can be used for extra debugging
+def create_min_max_markers(label, bbox, frame_id="hero/RADAR",
+                           min_color=(0.0, 1.0, 0.0, 1.0),
+                           max_color=(1.0, 0.0, 0.0, 1.0)):
     """
-    Erstellt RViz-Marker für die Min- und Max-Punkte einer Bounding Box.
+    creates RViz-Markers for min- and max-points of a bounding box.
 
     Args:
-        label (int): Die ID des Clusters (wird als Marker-ID genutzt).
-        bbox (tuple): Min- und Max-Werte der Bounding Box (x_min, x_max, y_min, y_max, z_min, z_max).
-        frame_id (str): Frame ID, in dem die Marker gezeichnet werden.
-        min_color (tuple): RGBA-Farbwerte für den Min-Punkt-Marker.
-        max_color (tuple): RGBA-Farbwerte für den Max-Punkt-Marker.
+        label (int): cluster-id (used as marker-ID in rviz).
+        bbox (tuple): min- and max-values of bounding box 
+        (x_min, x_max, y_min, y_max, z_min, z_max).
+        frame_id (str): frame ID for markers
+        min_color (tuple): RGBA-value for min-point-marker
+        max_color (tuple): RGBA-value for max-point-marker
 
     Returns:
-        tuple: Ein Paar von Markern (min_marker, max_marker).
+        tuple: pair of markers (min_marker, max_marker).
     """
     x_min, x_max, y_min, y_max, z_min, z_max = bbox
 
-    # marker = Marker()
-    # marker.header.frame_id = "hero/RADAR"
-    # marker.id = int(label)
-    # # marker.type = Marker.LINE_STRIP
-    # marker.type = Marker.LINE_LIST
-    # marker.action = Marker.ADD
-    # marker.scale.x = 0.1
-    # marker.color.r = 1.0
-    # marker.color.g = 1.0
-    # marker.color.b = 0.0
-    # marker.color.a = 1.0
-
-    # Min-Punkt-Marker
+    # min-point-marker
     min_marker = Marker()
     min_marker.header.frame_id = frame_id
-    min_marker.id = int(label * 10)  # ID für Min-Punkt
+    min_marker.id = int(label * 10) 
     min_marker.type = Marker.SPHERE
     min_marker.action = Marker.ADD
-    min_marker.scale.x = 0.2  # Größe des Punktes
+    min_marker.scale.x = 0.2 
     min_marker.scale.y = 0.2
     min_marker.scale.z = 0.2
     min_marker.color.r = min_color[0]
@@ -421,10 +424,10 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1
     min_marker.pose.position.y = y_min
     min_marker.pose.position.z = z_min
 
-    # Max-Punkt-Marker
+    # max-point-marker
     max_marker = Marker()
     max_marker.header.frame_id = frame_id
-    max_marker.id = int(label * 10 + 1)  # ID für Max-Punkt
+    max_marker.id = int(label * 10 + 1)
     max_marker.type = Marker.SPHERE
     max_marker.action = Marker.ADD
     max_marker.scale.x = 0.2
@@ -442,15 +445,45 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR", min_color=(0.0, 1
 
 
 def clear_old_markers(marker_array, max_id):
-    """Löscht alte Marker aus dem MarkerArray."""
+    """
+    Removes old markers from the given MarkerArray by setting the action 
+    to DELETE for markers with an ID greater than or equal to max_id.
+
+    Args:
+        marker_array (MarkerArray): The current MarkerArray containing all markers.
+        max_id (int): The highest ID of the new markers. Markers with an ID 
+                      greater than or equal to this value will be marked for deletion.
+
+    Returns:
+        MarkerArray: The updated MarkerArray with old markers removed.
+    """
     for marker in marker_array.markers:
         if marker.id >= max_id:
             marker.action = Marker.DELETE
     return marker_array
 
 
-# generates string with label-id and cluster size
-def generate_cluster_labels_and_colors(clusters, data, marker_array, bounding_boxes):
+# generates string with label-id and cluster size, can be used for extra debugging
+def generate_cluster_info(clusters, data, marker_array, bounding_boxes):
+    """
+    Generates information about clusters, including the label, number of points,
+    markers, and bounding boxes.
+
+    Args:
+        clusters (DBSCAN): The clustered data, containing the labels for each point.
+        data (numpy.ndarray):
+        The point cloud data, typically with columns [x, y, z, distance].
+        marker_array (MarkerArray):
+        The array of RViz markers associated with the clusters.
+        bounding_boxes (list): The list of bounding boxes for each detected object.
+
+    Returns:
+        str: A JSON string containing the information about each cluster, including:
+             - "label": The cluster label.
+             - "points_count": The number of points in the cluster.
+             - "Anzahl marker": The number of markers in the MarkerArray.
+             - "Anzahl Boundingboxen": The number of bounding boxes.
+    """
     cluster_info = []
 
     for label in set(clusters.labels_):

From 57e720d6119685ac63d3e276dcbb11c9c4a59aaa Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 11 Dec 2024 15:23:41 +0100
Subject: [PATCH 06/10] removed trailing white spaces

---
 code/perception/src/radar_node.py | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 975af2b1..aecbeca2 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -130,8 +130,8 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma
     Points outside these bounds are excluded from the output.
 
     Args:
-        data (np.ndarray): A 2D numpy array containing radar data, where each row 
-        represents a data point with the format [x, y, z, distance]. The array 
+        data (np.ndarray): A 2D numpy array containing radar data, where each row
+        represents a data point with the format [x, y, z, distance]. The array
         shape is (N, 4), where N is the number of points.
         min_x (float, optional): Minimum value for the x-coordinate. Default is -1.
         max_x (float, optional): Maximum value for the x-coordinate. Default is 1.
@@ -143,7 +143,7 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma
         the sensor. Default is 100.
 
     Returns:
-        np.ndarray: A numpy array containing only the filtered data points that meet 
+        np.ndarray: A numpy array containing only the filtered data points that meet
         the specified criteria.
     """
 
@@ -257,7 +257,7 @@ def calculate_aabb(cluster_points):
     all points in the cluster.
 
     Args:
-        cluster_points (numpy.ndarray): 
+        cluster_points (numpy.ndarray):
         A 2D array where each row represents a 3D point (x, y, z).
         The array should have shape (N, 3) where N is the number of points.
 
@@ -396,7 +396,7 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR",
 
     Args:
         label (int): cluster-id (used as marker-ID in rviz).
-        bbox (tuple): min- and max-values of bounding box 
+        bbox (tuple): min- and max-values of bounding box
         (x_min, x_max, y_min, y_max, z_min, z_max).
         frame_id (str): frame ID for markers
         min_color (tuple): RGBA-value for min-point-marker
@@ -410,10 +410,10 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR",
     # min-point-marker
     min_marker = Marker()
     min_marker.header.frame_id = frame_id
-    min_marker.id = int(label * 10) 
+    min_marker.id = int(label * 10)
     min_marker.type = Marker.SPHERE
     min_marker.action = Marker.ADD
-    min_marker.scale.x = 0.2 
+    min_marker.scale.x = 0.2
     min_marker.scale.y = 0.2
     min_marker.scale.z = 0.2
     min_marker.color.r = min_color[0]
@@ -446,12 +446,12 @@ def create_min_max_markers(label, bbox, frame_id="hero/RADAR",
 
 def clear_old_markers(marker_array, max_id):
     """
-    Removes old markers from the given MarkerArray by setting the action 
+    Removes old markers from the given MarkerArray by setting the action
     to DELETE for markers with an ID greater than or equal to max_id.
 
     Args:
         marker_array (MarkerArray): The current MarkerArray containing all markers.
-        max_id (int): The highest ID of the new markers. Markers with an ID 
+        max_id (int): The highest ID of the new markers. Markers with an ID
                       greater than or equal to this value will be marked for deletion.
 
     Returns:

From 0f1a9f93b785d26cfa6945c2c7be8fb00a2b8358 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 11 Dec 2024 15:47:18 +0100
Subject: [PATCH 07/10] bug fixes

---
 code/perception/src/radar_node.py | 28 +++++++++++++++-------------
 1 file changed, 15 insertions(+), 13 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index aecbeca2..246e86cf 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -74,21 +74,21 @@ def listener(self):
         )
         self.visualization_radar_publisher = rospy.Publisher(
             rospy.get_param(
-                "~image_distance_topic", "/paf/hero/Radar/Visualization"
+                "~visualisation_topic", "/paf/hero/Radar/Visualization"
             ),
             PointCloud2,
             queue_size=10,
         )
         self.marker_visualization_radar_publisher = rospy.Publisher(
             rospy.get_param(
-                "~image_distance_topic", "/paf/hero/Radar/Marker"
+                "~marker_topic", "/paf/hero/Radar/Marker"
             ),
             MarkerArray,
             queue_size=10,
         )
         self.cluster_info_radar_publisher = rospy.Publisher(
             rospy.get_param(
-                "~image_distance_topic", "/paf/hero/Radar/ClusterInfo"
+                "~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo"
             ),
             String,
             queue_size=10,
@@ -133,12 +133,12 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma
         data (np.ndarray): A 2D numpy array containing radar data, where each row
         represents a data point with the format [x, y, z, distance]. The array
         shape is (N, 4), where N is the number of points.
-        min_x (float, optional): Minimum value for the x-coordinate. Default is -1.
-        max_x (float, optional): Maximum value for the x-coordinate. Default is 1.
-        min_y (float, optional): Minimum value for the y-coordinate. Default is 1.
-        max_y (float, optional): Maximum value for the y-coordinate. Default is 1.
-        min_z (float, optional): Minimum value for the z-coordinate. Default is -0.7.
-        max_z (float, optional): Maximum value for the z-coordinate. Default is 1.3.
+        min_x (float, optional): Minimum value for the x-coordinate. Default is -100.
+        max_x (float, optional): Maximum value for the x-coordinate. Default is 100.
+        min_y (float, optional): Minimum value for the y-coordinate. Default is -100.
+        max_y (float, optional): Maximum value for the y-coordinate. Default is 100.
+        min_z (float, optional): Minimum value for the z-coordinate. Default is -1.
+        max_z (float, optional): Maximum value for the z-coordinate. Default is 100.
         max_distance (float, optional): Maximum allowable distance of the point from
         the sensor. Default is 100.
 
@@ -160,7 +160,8 @@ def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, ma
 
 
 def cluster_data(data, eps=0.8, min_samples=3):
-    """_summary_
+    """
+    Clusters the radar data using the DBSCAN algorithm
 
     Args:
         data (np.ndarray): data array which should be clustered
@@ -171,6 +172,7 @@ def cluster_data(data, eps=0.8, min_samples=3):
         dict: 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.
+        DBSCAN: A DBSCAN clustering object containing labels and core sample indices
     """
 
     if len(data) == 0:
@@ -458,7 +460,7 @@ def clear_old_markers(marker_array, max_id):
         MarkerArray: The updated MarkerArray with old markers removed.
     """
     for marker in marker_array.markers:
-        if marker.id >= max_id:
+        if marker.id > max_id:
             marker.action = Marker.DELETE
     return marker_array
 
@@ -493,8 +495,8 @@ def generate_cluster_info(clusters, data, marker_array, bounding_boxes):
             cluster_info.append({
                 "label": int(label),
                 "points_count": cluster_size,
-                "Anzahl marker": len(marker_array.markers),
-                "Anzahl Boundingboxen": len(bounding_boxes)
+                "num_marker": len(marker_array.markers),
+                "num_bounding_boxes": len(bounding_boxes)
             })
 
     return json.dumps(cluster_info)

From 40a4595c977687321b7b3d63d5b077b218b6cc86 Mon Sep 17 00:00:00 2001
From: Ralf <ralf.bernitt@uni-a.de>
Date: Wed, 11 Dec 2024 15:55:29 +0100
Subject: [PATCH 08/10] added black formatter extension

---
 code/perception/src/radar_node.py | 83 +++++++++++++++++++------------
 1 file changed, 51 insertions(+), 32 deletions(-)

diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py
index 246e86cf..fcb32ddd 100755
--- a/code/perception/src/radar_node.py
+++ b/code/perception/src/radar_node.py
@@ -40,8 +40,9 @@ def callback(self, data):
         cloud = create_pointcloud2(dataarray, clustered_data.labels_)
         self.visualization_radar_publisher.publish(cloud)
 
-        points_with_labels = np.hstack((dataarray,
-                                        clustered_data.labels_.reshape(-1, 1)))
+        points_with_labels = np.hstack(
+            (dataarray, clustered_data.labels_.reshape(-1, 1))
+        )
         bounding_boxes = generate_bounding_boxes(points_with_labels)
 
         marker_array = MarkerArray()
@@ -58,8 +59,9 @@ def callback(self, data):
 
         self.marker_visualization_radar_publisher.publish(marker_array)
 
-        cluster_info = generate_cluster_info(clustered_data, dataarray, marker_array,
-                                             bounding_boxes)
+        cluster_info = generate_cluster_info(
+            clustered_data, dataarray, marker_array, bounding_boxes
+        )
         self.cluster_info_radar_publisher.publish(cluster_info)
 
     def listener(self):
@@ -73,23 +75,17 @@ def listener(self):
             queue_size=10,
         )
         self.visualization_radar_publisher = rospy.Publisher(
-            rospy.get_param(
-                "~visualisation_topic", "/paf/hero/Radar/Visualization"
-            ),
+            rospy.get_param("~visualisation_topic", "/paf/hero/Radar/Visualization"),
             PointCloud2,
             queue_size=10,
         )
         self.marker_visualization_radar_publisher = rospy.Publisher(
-            rospy.get_param(
-                "~marker_topic", "/paf/hero/Radar/Marker"
-            ),
+            rospy.get_param("~marker_topic", "/paf/hero/Radar/Marker"),
             MarkerArray,
             queue_size=10,
         )
         self.cluster_info_radar_publisher = rospy.Publisher(
-            rospy.get_param(
-                "~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo"
-            ),
+            rospy.get_param("~clusterInfo_topic_topic", "/paf/hero/Radar/ClusterInfo"),
             String,
             queue_size=10,
         )
@@ -121,8 +117,16 @@ def pointcloud2_to_array(pointcloud_msg):
     )
 
 
-def filter_data(data, min_x=-100, max_x=100, min_y=-100, max_y=100, min_z=-1, max_z=100,
-                max_distance=100):
+def filter_data(
+    data,
+    min_x=-100,
+    max_x=100,
+    min_y=-100,
+    max_y=100,
+    min_z=-1,
+    max_z=100,
+    max_distance=100,
+):
     """
     Filters radar data based on specified spatial and distance constraints.
 
@@ -218,14 +222,14 @@ def create_pointcloud2(clustered_points, cluster_labels):
         else:
             r, g, b = colors[label]
 
-        rgb = struct.unpack('f', struct.pack('I', (r << 16) | (g << 8) | b))[0]
+        rgb = struct.unpack("f", struct.pack("I", (r << 16) | (g << 8) | b))[0]
         points.append([x, y, z, rgb])
 
     fields = [
-        PointField('x', 0, PointField.FLOAT32, 1),
-        PointField('y', 4, PointField.FLOAT32, 1),
-        PointField('z', 8, PointField.FLOAT32, 1),
-        PointField('rgb', 12, PointField.FLOAT32, 1),
+        PointField("x", 0, PointField.FLOAT32, 1),
+        PointField("y", 4, PointField.FLOAT32, 1),
+        PointField("z", 8, PointField.FLOAT32, 1),
+        PointField("rgb", 12, PointField.FLOAT32, 1),
     ]
 
     return point_cloud2.create_cloud(header, fields, points)
@@ -378,9 +382,18 @@ def create_bounding_box_marker(label, bbox):
     ]
 
     lines = [
-        (0, 1), (1, 2), (2, 3), (3, 0),  # Bottom
-        (4, 5), (5, 6), (6, 7), (7, 4),  # Top
-        (0, 4), (1, 5), (2, 6), (3, 7),  # Vertical Edges
+        (0, 1),
+        (1, 2),
+        (2, 3),
+        (3, 0),  # Bottom
+        (4, 5),
+        (5, 6),
+        (6, 7),
+        (7, 4),  # Top
+        (0, 4),
+        (1, 5),
+        (2, 6),
+        (3, 7),  # Vertical Edges
     ]
     for start, end in lines:
         marker.points.append(points[start])
@@ -390,9 +403,13 @@ def create_bounding_box_marker(label, bbox):
 
 
 # can be used for extra debugging
-def create_min_max_markers(label, bbox, frame_id="hero/RADAR",
-                           min_color=(0.0, 1.0, 0.0, 1.0),
-                           max_color=(1.0, 0.0, 0.0, 1.0)):
+def create_min_max_markers(
+    label,
+    bbox,
+    frame_id="hero/RADAR",
+    min_color=(0.0, 1.0, 0.0, 1.0),
+    max_color=(1.0, 0.0, 0.0, 1.0),
+):
     """
     creates RViz-Markers for min- and max-points of a bounding box.
 
@@ -492,12 +509,14 @@ def generate_cluster_info(clusters, data, marker_array, bounding_boxes):
         cluster_points = data[clusters.labels_ == label]
         cluster_size = len(cluster_points)
         if label != -1:
-            cluster_info.append({
-                "label": int(label),
-                "points_count": cluster_size,
-                "num_marker": len(marker_array.markers),
-                "num_bounding_boxes": len(bounding_boxes)
-            })
+            cluster_info.append(
+                {
+                    "label": int(label),
+                    "points_count": cluster_size,
+                    "num_marker": len(marker_array.markers),
+                    "num_bounding_boxes": len(bounding_boxes),
+                }
+            )
 
     return json.dumps(cluster_info)
 

From 486e5bf62e4f8112c1dd6f805e89e80c6d31b04a Mon Sep 17 00:00:00 2001
From: SirMDA <mail@ludwigholl.de>
Date: Thu, 12 Dec 2024 17:16:24 +0100
Subject: [PATCH 09/10] fixed lidar_distance node speed by removing for loop

---
 code/perception/src/lidar_distance.py | 129 ++++++++++++++++----------
 1 file changed, 79 insertions(+), 50 deletions(-)
 mode change 100644 => 100755 code/perception/src/lidar_distance.py

diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py
old mode 100644
new mode 100755
index 939224c5..bea1d550
--- a/code/perception/src/lidar_distance.py
+++ b/code/perception/src/lidar_distance.py
@@ -3,7 +3,7 @@
 import rospy
 import ros_numpy
 import numpy as np
-import lidar_filter_utility
+from lidar_filter_utility import bounding_box, remove_field_name
 from sensor_msgs.msg import PointCloud2, Image as ImageMsg
 from sklearn.cluster import DBSCAN
 from cv_bridge import CvBridge
@@ -189,14 +189,12 @@ def calculate_image(self, coordinates, focus):
             return None
 
         # Apply bounding box filter
-        reconstruct_bit_mask = lidar_filter_utility.bounding_box(coordinates, **params)
+        reconstruct_bit_mask = bounding_box(coordinates, **params)
         reconstruct_coordinates = coordinates[reconstruct_bit_mask]
 
         # Remove the "intensity" field and convert to a NumPy array
         reconstruct_coordinates_xyz = np.array(
-            lidar_filter_utility.remove_field_name(
-                reconstruct_coordinates, "intensity"
-            ).tolist()
+            remove_field_name(reconstruct_coordinates, "intensity").tolist()
         )
 
         # Reconstruct the image based on the focus
@@ -256,51 +254,82 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
         img = np.zeros(shape=(720, 1280), dtype=np.float32)
         dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32)
 
-        # Process each point in the point cloud
-        for c in coordinates_xyz:
-            if focus == "Center":  # Compute image for the center view
-                point = np.array([c[1], c[2], c[0], 1])
-                pixel = np.matmul(m, point)  # Project 3D point to 2D image coordinates
-                x, y = int(pixel[0] / pixel[2]), int(
-                    pixel[1] / pixel[2]
-                )  # Normalize coordinates
-                if (
-                    0 <= x <= 1280 and 0 <= y <= 720
-                ):  # Check if coordinates are within image bounds
-                    img[719 - y][1279 - x] = c[0]  # Set depth value
-                    dist_array[719 - y][1279 - x] = np.array(
-                        [c[0], c[1], c[2]], dtype=np.float32
-                    )
-
-            if focus == "Back":  # Compute image for the rear view
-                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 0 <= x <= 1280 and 0 <= y < 720:
-                    img[y][1279 - x] = -c[0]
-                    dist_array[y][1279 - x] = np.array(
-                        [-c[0], c[1], c[2]], dtype=np.float32
-                    )
-
-            if focus == "Left":  # Compute image for the left view
-                point = np.array([c[0], c[2], c[1], 1])
-                pixel = np.matmul(m, point)
-                x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2])
-                if 0 <= x <= 1280 and 0 <= y <= 720:
-                    img[719 - y][1279 - x] = c[1]
-                    dist_array[y][1279 - x] = np.array(
-                        [c[0], c[1], c[2]], dtype=np.float32
-                    )
-
-            if focus == "Right":  # Compute image for the right view
-                point = np.array([c[0], c[2], c[1], 1])
-                pixel = np.matmul(m, point)
-                x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2])
-                if 0 <= x < 1280 and 0 <= y < 720:
-                    img[y][1279 - x] = -c[1]
-                    dist_array[y][1279 - x] = np.array(
-                        [c[0], c[1], c[2]], dtype=np.float32
-                    )
+        # Prepare points based on focus
+        if focus == "Center":
+            points = np.column_stack(
+                (
+                    coordinates_xyz[:, 1],
+                    coordinates_xyz[:, 2],
+                    coordinates_xyz[:, 0],
+                    np.ones(coordinates_xyz.shape[0]),
+                )
+            )
+        elif focus == "Back":
+            points = np.column_stack(
+                (
+                    coordinates_xyz[:, 1],
+                    coordinates_xyz[:, 2],
+                    coordinates_xyz[:, 0],
+                    np.ones(coordinates_xyz.shape[0]),
+                )
+            )
+        elif focus == "Left":
+            points = np.column_stack(
+                (
+                    coordinates_xyz[:, 0],
+                    coordinates_xyz[:, 2],
+                    coordinates_xyz[:, 1],
+                    np.ones(coordinates_xyz.shape[0]),
+                )
+            )
+        elif focus == "Right":
+            points = np.column_stack(
+                (
+                    coordinates_xyz[:, 0],
+                    coordinates_xyz[:, 2],
+                    coordinates_xyz[:, 1],
+                    np.ones(coordinates_xyz.shape[0]),
+                )
+            )
+        else:
+            rospy.logwarn(f"Unknown focus: {focus}. Skipping image calculation.")
+            return None
+
+        # Project 3D points to 2D image coordinates
+        pixels = np.dot(m, points.T).T
+        x = (pixels[:, 0] / pixels[:, 2]).astype(int)
+        y = (pixels[:, 1] / pixels[:, 2]).astype(int)
+
+        # Filter valid coordinates
+        valid_indices = (x >= 0) & (x < 1280) & (y >= 0) & (y < 720)
+        x = x[valid_indices]
+        y = y[valid_indices]
+        valid_coordinates = coordinates_xyz[valid_indices]
+
+        if focus == "Center":
+            img[719 - y, 1279 - x] = valid_coordinates[:, 0]
+            dist_array[719 - y, 1279 - x] = valid_coordinates
+        elif focus == "Back":
+            img[y, 1279 - x] = -valid_coordinates[:, 0]
+            dist_array[y, 1279 - x] = np.column_stack(
+                (
+                    -valid_coordinates[:, 0],
+                    valid_coordinates[:, 1],
+                    valid_coordinates[:, 2],
+                )
+            )
+        elif focus == "Left":
+            img[719 - y, 1279 - x] = valid_coordinates[:, 1]
+            dist_array[719 - y, 1279 - x] = valid_coordinates
+        elif focus == "Right":
+            img[y, 1279 - x] = -valid_coordinates[:, 1]
+            dist_array[y, 1279 - x] = np.column_stack(
+                (
+                    valid_coordinates[:, 0],
+                    -valid_coordinates[:, 1],
+                    valid_coordinates[:, 2],
+                )
+            )
 
         return dist_array
 

From eefcd29f6690efbfa765c140a77261c161386f05 Mon Sep 17 00:00:00 2001
From: SirMDA <mail@ludwigholl.de>
Date: Thu, 12 Dec 2024 17:33:37 +0100
Subject: [PATCH 10/10] optimized focus if clause

---
 code/perception/src/lidar_distance.py | 22 ++--------------------
 1 file changed, 2 insertions(+), 20 deletions(-)

diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py
index bea1d550..5c937be0 100755
--- a/code/perception/src/lidar_distance.py
+++ b/code/perception/src/lidar_distance.py
@@ -255,16 +255,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
         dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32)
 
         # Prepare points based on focus
-        if focus == "Center":
-            points = np.column_stack(
-                (
-                    coordinates_xyz[:, 1],
-                    coordinates_xyz[:, 2],
-                    coordinates_xyz[:, 0],
-                    np.ones(coordinates_xyz.shape[0]),
-                )
-            )
-        elif focus == "Back":
+        if focus in ["Center", "Back"]:
             points = np.column_stack(
                 (
                     coordinates_xyz[:, 1],
@@ -273,16 +264,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
                     np.ones(coordinates_xyz.shape[0]),
                 )
             )
-        elif focus == "Left":
-            points = np.column_stack(
-                (
-                    coordinates_xyz[:, 0],
-                    coordinates_xyz[:, 2],
-                    coordinates_xyz[:, 1],
-                    np.ones(coordinates_xyz.shape[0]),
-                )
-            )
-        elif focus == "Right":
+        elif focus in ["Left", "Right"]:
             points = np.column_stack(
                 (
                     coordinates_xyz[:, 0],