diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index 5c937be0..6bc33305 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -7,6 +7,8 @@ from sensor_msgs.msg import PointCloud2, Image as ImageMsg from sklearn.cluster import DBSCAN from cv_bridge import CvBridge +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Point # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations @@ -60,14 +62,6 @@ def listener(self): ImageMsg, queue_size=10, ) - self.dist_array_lidar_publisher = rospy.Publisher( - rospy.get_param( - "~image_distance_topic_cluster", "/paf/hero/dist_clustered" - ), - PointCloud2, - queue_size=10, - ) - rospy.loginfo("dist_array_lidar_publisher successfully created.") self.dist_array_left_publisher = rospy.Publisher( rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), ImageMsg, @@ -78,6 +72,18 @@ def listener(self): ImageMsg, queue_size=10, ) + self.dist_array_lidar_publisher = rospy.Publisher( + rospy.get_param( + "~image_distance_topic_cluster", "/paf/hero/dist_clustered" + ), + PointCloud2, + queue_size=10, + ) + self.marker_visualization_lidar_publisher = rospy.Publisher( + rospy.get_param("~marker_topic", "/paf/hero/Lidar/Marker"), + MarkerArray, + queue_size=10, + ) # Subscriber for LiDAR data (point clouds) rospy.Subscriber( @@ -92,42 +98,67 @@ def listener(self): def start_clustering(self, data): """ Filters LiDAR point clouds, performs clustering, - and publishes the combined clusters. + generates bounding boxes, and publishes the results. :param data: LiDAR point clouds in ROS PointCloud2 format. """ - # Filter point clouds to remove irrelevant data + # Convert PointCloud2 data to a NumPy structured array coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + + # Filter the point clouds to exclude irrelevant data filtered_coordinates = coordinates[ ~( (coordinates["x"] >= -2) - & (coordinates["x"] <= 2) + & (coordinates["x"] <= 2) # Exclude ego vehicle in x-axis & (coordinates["y"] >= -1) - & (coordinates["y"] <= 1) - ) # Exclude points related to the ego vehicle + & (coordinates["y"] <= 1) # Exclude ego vehicle in y-axis + ) & ( - coordinates["z"] > -1.7 + 0.05 - ) # Minimum height in z to exclude the road + coordinates["z"] > -1.7 + 0.3 + ) # Exclude points below a certain height (street) ] - # Compute cluster data from the filtered coordinates - clustered_points = cluster_lidar_data_from_pointcloud( + # Perform clustering on the filtered coordinates + clustered_points, cluster_labels = cluster_lidar_data_from_pointcloud( coordinates=filtered_coordinates ) - # Only store valid cluster data + # Extract x, y, z coordinates into a separate array + filtered_xyz = np.column_stack( + ( + filtered_coordinates["x"], + filtered_coordinates["y"], + filtered_coordinates["z"], + ) + ) + + # Combine coordinates with their cluster labels + cluster_labels = cluster_labels.reshape(-1, 1) + points_with_labels = np.hstack((filtered_xyz, cluster_labels)) + + bounding_boxes = generate_bounding_boxes(points_with_labels) + + # Create a MarkerArray for visualization + marker_array = MarkerArray() + for label, bbox in bounding_boxes: + if label != -1: # Ignore noise points (label = -1) + marker = create_bounding_box_marker(label, bbox) + marker_array.markers.append(marker) + + # Publish the MarkerArray for visualization + self.marker_visualization_lidar_publisher.publish(marker_array) + + # Store valid cluster data for combining if clustered_points: self.cluster_buffer.append(clustered_points) else: rospy.logwarn("No cluster data generated.") - # Combine clusters + # Combine clusters from the buffer combined_clusters = combine_clusters(self.cluster_buffer) - self.cluster_buffer = [] - # Publish the combined clusters self.publish_clusters(combined_clusters, data.header) def publish_clusters(self, combined_clusters, data_header): @@ -316,6 +347,154 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): return dist_array +def generate_bounding_boxes(points_with_labels): + """ + Generates axis-aligned bounding boxes (AABB) for clustered points. + + This function calculates bounding boxes for each unique + cluster label in the input array. + + Args: + points_with_labels (numpy.ndarray): + A 2D array of shape (N, 4), where each row contains: + - Coordinates (x, y, z) of a point + - Cluster label in the last column + Structure: [x, y, z, label] + + Returns: + list: + A list of tuples. Each tuple contains: + - A cluster label (int/float) + - A bounding box (tuple): (x_min, x_max, y_min, y_max, z_min, z_max) + """ + bounding_boxes = [] + + # Identify unique cluster labels + unique_labels = np.unique(points_with_labels[:, -1]) + + # Process each cluster label + for label in unique_labels: + if label == -1: # Skip noise points (label = -1) + continue + + # Extract points belonging to the current cluster + cluster_points = points_with_labels[points_with_labels[:, -1] == label, :3] + + # Calculate the bounding box for the cluster + bbox = calculate_aabb(cluster_points) + bounding_boxes.append((label, bbox)) + + return bounding_boxes + + +def create_bounding_box_marker(label, bbox): + """ + Creates an RViz Marker for visualizing a 3D bounding box. + + This function generates a Marker object for RViz to visualize a 3D bounding box + as a series of connected lines representing the edges of the box. + + Args: + label (int): Unique identifier for the cluster or object. + Used as the Marker ID. + bbox (tuple): Bounding box dimensions in the format: + (x_min, x_max, y_min, y_max, z_min, z_max). + + Returns: + Marker: A LINE_LIST Marker object that can be published to RViz. + """ + x_min, x_max, y_min, y_max, z_min, z_max = bbox + + # Initialize the Marker object + marker = Marker() + marker.header.frame_id = "hero/LIDAR" # Reference frame for the marker + marker.ns = "marker_lidar" # Namespace to group related markers + marker.id = int(label) # Use the label as the unique marker ID + marker.lifetime = rospy.Duration(0.1) # Marker visibility duration in seconds + marker.type = Marker.LINE_LIST # Marker type to represent bounding box edges + marker.action = Marker.ADD # Action to add or modify the marker + + # Set marker properties + marker.scale.x = 0.1 # Line thickness + marker.color.r = 1.0 # Red color component + marker.color.g = 0.5 # Green color component + marker.color.b = 0.5 # Blue color component + marker.color.a = 1.0 # Opacity (1.0 = fully visible) + + # Define the 8 corners of the 3D bounding box + points = [ + Point(x_min, y_min, z_min), # Bottom face + 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), # Top face + Point(x_max, y_min, z_max), + Point(x_max, y_max, z_max), + Point(x_min, y_max, z_max), + ] + + # Define lines connecting the corners of the bounding box + lines = [ + (0, 1), + (1, 2), + (2, 3), + (3, 0), # Bottom face + (4, 5), + (5, 6), + (6, 7), + (7, 4), # Top face + (0, 4), + (1, 5), + (2, 6), + (3, 7), # Vertical edges + ] + + # Add points for each line segment to the marker + for start, end in lines: + marker.points.append(points[start]) + marker.points.append(points[end]) + + return marker + + +def calculate_aabb(cluster_points): + """ + 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 (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: + 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 + # 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]) + # 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 + 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]) + return x_min, x_max, y_min, y_max, z_min, z_max + + def array_to_pointcloud2(points, header="hero/Lidar"): """ Converts an array of points into a ROS PointCloud2 message. @@ -384,7 +563,7 @@ def combine_clusters(cluster_buffer): return combined_points -def cluster_lidar_data_from_pointcloud(coordinates, eps=0.3, min_samples=10): +def cluster_lidar_data_from_pointcloud(coordinates, eps=0.4, min_samples=10): """ Performs clustering on LiDAR data using DBSCAN and returns the clusters. @@ -422,7 +601,7 @@ def cluster_lidar_data_from_pointcloud(coordinates, eps=0.3, min_samples=10): clusters = dict(clusters) - return clusters + return clusters, labels if __name__ == "__main__": diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index fcb32ddd..7f20f4ef 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -31,7 +31,7 @@ def callback(self, data): dataarray = pointcloud2_to_array(data) # radar position z=0.7 - dataarray = filter_data(dataarray, min_z=-0.6) + dataarray = filter_data(dataarray, min_z=-0.40, max_z=2) clustered_data = cluster_data(dataarray) @@ -55,8 +55,6 @@ def callback(self, data): # 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_info( @@ -163,7 +161,7 @@ def filter_data( return filtered_data -def cluster_data(data, eps=0.8, min_samples=3): +def cluster_data(data, eps=0.4, min_samples=3): """ Clusters the radar data using the DBSCAN algorithm @@ -349,7 +347,9 @@ def create_bounding_box_marker(label, bbox): marker = Marker() marker.header.frame_id = "hero/RADAR" + marker.ns = "marker_radar" marker.id = int(label) + marker.lifetime = rospy.Duration(1) # marker.type = Marker.LINE_STRIP # 2d boxes marker.type = Marker.LINE_LIST # 3d boxes marker.action = Marker.ADD @@ -463,25 +463,6 @@ def create_min_max_markers( return min_marker, max_marker -def clear_old_markers(marker_array, max_id): - """ - 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, can be used for extra debugging def generate_cluster_info(clusters, data, marker_array, bounding_boxes): """