From 634f678a5df379c314c326457bc8367ba0a23477 Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 13 Nov 2024 12:50:37 +0100 Subject: [PATCH 1/7] Add first draft of radar node --- code/perception/launch/perception.launch | 6 ++ code/perception/src/radar_node.py | 97 ++++++++++++++++++++++++ 2 files changed, 103 insertions(+) create mode 100755 code/perception/src/radar_node.py diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 8d6072e7..302a97d6 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -79,4 +79,10 @@ + + + + + + diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py new file mode 100755 index 00000000..b35060c1 --- /dev/null +++ b/code/perception/src/radar_node.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +import numpy as np +import lidar_filter_utility +from sensor_msgs.msg import PointCloud2 + +# from mpl_toolkits.mplot3d import Axes3D +# from itertools import combinations +from sensor_msgs.msg import Image as ImageMsg +from cv_bridge import CvBridge + +# from matplotlib.colors import LinearSegmentedColormap + + +class RadarNode: + """See doc/perception/lidar_distance_utility.md on + how to configute this node + """ + + def callback(self, data): + """Callback function, filters a PontCloud2 message + by restrictions defined in the launchfile. + + Publishes a Depth image for the specified camera angle. + Each angle has do be delt with differently since the signs of the + coordinate system change with the view angle. + + :param data: a PointCloud2 + """ + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + + # Center + reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( + coordinates, + max_x=np.inf, + min_x=0.0, + min_z=-1.6, + ) + reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] + reconstruct_coordinates_xyz_center = np.array( + lidar_filter_utility.remove_field_name( + reconstruct_coordinates_center, "intensity" + ).tolist() + ) + dist_array_center = self.reconstruct_img_from_lidar( + reconstruct_coordinates_xyz_center, focus="Center" + ) + dist_array_center_msg = self.bridge.cv2_to_imgmsg( + dist_array_center, encoding="passthrough" + ) + dist_array_center_msg.header = data.header + self.dist_array_center_publisher.publish(dist_array_center_msg) + + def listener(self): + """ + Initializes the node and it's publishers + """ + # run simultaneously. + rospy.init_node("lidar_distance") + self.bridge = CvBridge() + + self.pub_pointcloud = rospy.Publisher( + rospy.get_param( + "~point_cloud_topic", + "/carla/hero/" + rospy.get_namespace() + "_filtered", + ), + PointCloud2, + queue_size=10, + ) + + # publisher for dist_array + self.dist_array_center_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), + ImageMsg, + queue_size=10, + ) + + # publisher for dist_array + self.dist_array_center_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), + ImageMsg, + queue_size=10, + ) + + rospy.Subscriber( + rospy.get_param("~source_topic", "/carla/hero/RADAR"), + PointCloud2, + self.callback, + ) + + rospy.spin() + + +if __name__ == "__main__": + lidar_distance = RadarNode() + lidar_distance.listener() \ No newline at end of file From d537803e1b86b44418f326c6fac171b20782c1e0 Mon Sep 17 00:00:00 2001 From: Ralf Date: Thu, 14 Nov 2024 12:05:39 +0100 Subject: [PATCH 2/7] update radar node --- code/perception/launch/perception.launch | 4 +- code/perception/src/radar_node.py | 90 ++++++++++++++++-------- 2 files changed, 61 insertions(+), 33 deletions(-) diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 302a97d6..8320e3f5 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -81,8 +81,8 @@ - - + + diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index b35060c1..bd8f351f 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -10,6 +10,8 @@ from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge +from std_msgs.msg import Float32 + # from matplotlib.colors import LinearSegmentedColormap @@ -28,29 +30,53 @@ def callback(self, data): :param data: a PointCloud2 """ - coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + # coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) # Center - reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( - coordinates, - max_x=np.inf, - min_x=0.0, - min_z=-1.6, - ) - reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] - reconstruct_coordinates_xyz_center = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates_center, "intensity" - ).tolist() - ) - dist_array_center = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_center, focus="Center" - ) - dist_array_center_msg = self.bridge.cv2_to_imgmsg( - dist_array_center, encoding="passthrough" - ) - dist_array_center_msg.header = data.header - self.dist_array_center_publisher.publish(dist_array_center_msg) + + # reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( + # coordinates, + # max_x=np.inf, + # min_x=0.0, + # min_z=-1.6, + # ) + # reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] + # reconstruct_coordinates_xyz_center = np.array( + # lidar_filter_utility.remove_field_name( + # reconstruct_coordinates_center, "intensity" + # ).tolist() + # ) + # dist_array_center = self.reconstruct_img_from_lidar( + # reconstruct_coordinates_xyz_center, focus="Center" + # ) + # dist_array_center_msg = self.bridge.cv2_to_imgmsg( + # dist_array_center, encoding="passthrough" + # ) + # dist_array_center_msg.header = data.header + # self.dist_array_center_publisher.publish(dist_array_center_msg) + + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + + # x_values = coordinates['x'] + # rospy.loginfo("Erste 5 x Werte: {x_values[:5]}") + + # depth_values = coordinates['depth'] + # rospy.loginfo("Erste 5 Depth Werte: {depth_values[:5]}") + + dtype_info = "\n".join([f"Feld '{name}': {coordinates.dtype[name]}" for name in coordinates.dtype.names]) + + # rospy.loginfo("DatentypenNeu: " + dtype_info) # funktioniert + + # rospy.loginfo("DatentypenAlt: " + coordinates.dtype) + + + + msg = np.min[coordinates['Range']] + + dtype_msginfo = ["{type(msg).__name__}"] + rospy.loginfo("DatentypenMsg: " + dtype_msginfo) + + self.dist_array_radar_publisher.publish(msg) def listener(self): """ @@ -70,16 +96,18 @@ def listener(self): ) # publisher for dist_array - self.dist_array_center_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), - ImageMsg, - queue_size=10, - ) - # publisher for dist_array - self.dist_array_center_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), - ImageMsg, + # self.dist_array_center_publisher = rospy.Publisher( + # rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), + # ImageMsg, + # queue_size=10, + # ) + + # publisher for radar dist_array + self.dist_array_radar_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Radar/array"), + # PointCloud2, + Float32, queue_size=10, ) @@ -94,4 +122,4 @@ def listener(self): if __name__ == "__main__": lidar_distance = RadarNode() - lidar_distance.listener() \ No newline at end of file + lidar_distance.listener() From 7ecd33e4d1f19a77d581331292543a3183b0c582 Mon Sep 17 00:00:00 2001 From: Ralf Date: Mon, 18 Nov 2024 17:08:25 +0100 Subject: [PATCH 3/7] update radar node --- code/perception/src/radar_node.py | 61 +++++-------------------------- 1 file changed, 10 insertions(+), 51 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index bd8f351f..b1a7d603 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -30,53 +30,29 @@ def callback(self, data): :param data: a PointCloud2 """ - # coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - - # Center - - # reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( - # coordinates, - # max_x=np.inf, - # min_x=0.0, - # min_z=-1.6, - # ) - # reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] - # reconstruct_coordinates_xyz_center = np.array( - # lidar_filter_utility.remove_field_name( - # reconstruct_coordinates_center, "intensity" - # ).tolist() - # ) - # dist_array_center = self.reconstruct_img_from_lidar( - # reconstruct_coordinates_xyz_center, focus="Center" - # ) - # dist_array_center_msg = self.bridge.cv2_to_imgmsg( - # dist_array_center, encoding="passthrough" - # ) - # dist_array_center_msg.header = data.header - # self.dist_array_center_publisher.publish(dist_array_center_msg) coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - # x_values = coordinates['x'] # rospy.loginfo("Erste 5 x Werte: {x_values[:5]}") # depth_values = coordinates['depth'] # rospy.loginfo("Erste 5 Depth Werte: {depth_values[:5]}") - dtype_info = "\n".join([f"Feld '{name}': {coordinates.dtype[name]}" for name in coordinates.dtype.names]) - + # dtype_info = "\n".join([f"Feld '{name}': {coordinates.dtype[name]}" for name in coordinates.dtype.names]) # rospy.loginfo("DatentypenNeu: " + dtype_info) # funktioniert - # rospy.loginfo("DatentypenAlt: " + coordinates.dtype) - - + #msg = np.min(coordinates['Velocity']) + # rospy.loginfo("DatentypenMsg: " + str(msg.dtype)) + # rospy.loginfo("WertMsg: " + str(msg)) - msg = np.min[coordinates['Range']] + #self.dist_array_radar_publisher.publish(msg) - dtype_msginfo = ["{type(msg).__name__}"] - rospy.loginfo("DatentypenMsg: " + dtype_msginfo) + for coordinate in coordinates: - self.dist_array_radar_publisher.publish(msg) + if coordinate['y'] == 0: + if coordinate['x'] <= 15: + msg = coordinate['Velocity']*3.6 + self.dist_array_radar_publisher.publish(msg) def listener(self): """ @@ -86,23 +62,6 @@ def listener(self): rospy.init_node("lidar_distance") self.bridge = CvBridge() - self.pub_pointcloud = rospy.Publisher( - rospy.get_param( - "~point_cloud_topic", - "/carla/hero/" + rospy.get_namespace() + "_filtered", - ), - PointCloud2, - queue_size=10, - ) - - # publisher for dist_array - - # self.dist_array_center_publisher = rospy.Publisher( - # rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), - # ImageMsg, - # queue_size=10, - # ) - # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( rospy.get_param("~image_distance_topic", "/paf/hero/Radar/array"), From 08e9711afb0c2ddb4d5d4f41def316f3216f429b Mon Sep 17 00:00:00 2001 From: Ralf Date: Wed, 20 Nov 2024 16:54:16 +0100 Subject: [PATCH 4/7] Aleks and Ralf, implemented first distance detection with radar_node --- code/perception/src/radar_node.py | 72 ++++++++++++++++++------------- 1 file changed, 43 insertions(+), 29 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index b1a7d603..1fa98f2e 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -4,14 +4,15 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2 - +from sklearn.cluster import DBSCAN # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge - -from std_msgs.msg import Float32 - +import json +# from std_msgs.msg import Float32 +from std_msgs.msg import String +from rospy.numpy_msg import numpy_msg # from matplotlib.colors import LinearSegmentedColormap @@ -30,29 +31,10 @@ def callback(self, data): :param data: a PointCloud2 """ - - coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) - # x_values = coordinates['x'] - # rospy.loginfo("Erste 5 x Werte: {x_values[:5]}") - - # depth_values = coordinates['depth'] - # rospy.loginfo("Erste 5 Depth Werte: {depth_values[:5]}") - - # dtype_info = "\n".join([f"Feld '{name}': {coordinates.dtype[name]}" for name in coordinates.dtype.names]) - # rospy.loginfo("DatentypenNeu: " + dtype_info) # funktioniert - - #msg = np.min(coordinates['Velocity']) - # rospy.loginfo("DatentypenMsg: " + str(msg.dtype)) - # rospy.loginfo("WertMsg: " + str(msg)) - - #self.dist_array_radar_publisher.publish(msg) - - for coordinate in coordinates: - - if coordinate['y'] == 0: - if coordinate['x'] <= 15: - msg = coordinate['Velocity']*3.6 - 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): """ @@ -64,9 +46,9 @@ def listener(self): # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( - rospy.get_param("~image_distance_topic", "/paf/hero/Radar/array"), + rospy.get_param("~image_distance_topic", "/paf/hero/Radar/dist_array_unsegmented"), # PointCloud2, - Float32, + String, queue_size=10, ) @@ -79,6 +61,38 @@ 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__": lidar_distance = RadarNode() lidar_distance.listener() From 0af4cc90c99b22e64cc1bcf269a01f085a02a3c6 Mon Sep 17 00:00:00 2001 From: Ralf Date: Thu, 21 Nov 2024 13:13:30 +0100 Subject: [PATCH 5/7] Add radar_node.md --- doc/perception/radar_node.md | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 doc/perception/radar_node.md diff --git a/doc/perception/radar_node.md b/doc/perception/radar_node.md new file mode 100644 index 00000000..26783bc2 --- /dev/null +++ b/doc/perception/radar_node.md @@ -0,0 +1,36 @@ +# Radar Node + +**Summary:** This page explains what the radar sensor does. + +- [Radar offsets](#radar-offsets) +- [Radar specification](#radar-specification) +- [Radar data output for each detected point](#radar-data-output-for-each-detected-point) +- [Todo](#todo) + +## Radar offsets + +- x: 2 +- y: 0 +- z: 0.7 + +## Radar specification + +- points per second: 1500 points generated by all lasers per second +- maximum range: 100 meters +- horizontal fov: 30 degrees +- vertical fov: 30 degrees + +## Radar data output for each detected point + +- x +- y +- z +- Range +- Velocity +- AzimuthAngle +- ElevationAngle + +## Todo + +- Discuss further processing of radar data +- Combine lidar, radar and camera data From ae01b8de9792bdd5a4edc5e90096355e55d89e98 Mon Sep 17 00:00:00 2001 From: Ralf Bernitt Date: Sun, 24 Nov 2024 18:57:00 +0100 Subject: [PATCH 6/7] finish radar node --- code/perception/src/radar_node.py | 54 ++++++------------------------- 1 file changed, 10 insertions(+), 44 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 1fa98f2e..9b54bd18 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -4,15 +4,14 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2 -from sklearn.cluster import DBSCAN + # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge -import json -# from std_msgs.msg import Float32 -from std_msgs.msg import String -from rospy.numpy_msg import numpy_msg + +from std_msgs.msg import Float32 + # from matplotlib.colors import LinearSegmentedColormap @@ -31,10 +30,10 @@ def callback(self, data): :param data: a PointCloud2 """ - - 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) + + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + msg = np.min(coordinates["Velocity"]) + self.dist_array_radar_publisher.publish(msg) def listener(self): """ @@ -46,9 +45,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"), - # PointCloud2, - String, + rospy.get_param("~image_distance_topic", "/paf/hero/Radar/velocity"), + Float32, queue_size=10, ) @@ -61,38 +59,6 @@ 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__": lidar_distance = RadarNode() lidar_distance.listener() From a8a585e3451169b9cf60365cf4b5a686916deef1 Mon Sep 17 00:00:00 2001 From: Ralf Bernitt Date: Sun, 24 Nov 2024 19:28:29 +0100 Subject: [PATCH 7/7] Bug fixes --- code/perception/src/radar_node.py | 32 +++++++++++-------------------- 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/code/perception/src/radar_node.py b/code/perception/src/radar_node.py index 9b54bd18..04a9d59b 100755 --- a/code/perception/src/radar_node.py +++ b/code/perception/src/radar_node.py @@ -2,33 +2,24 @@ import rospy import ros_numpy import numpy as np -import lidar_filter_utility from sensor_msgs.msg import PointCloud2 - -# from mpl_toolkits.mplot3d import Axes3D -# from itertools import combinations -from sensor_msgs.msg import Image as ImageMsg -from cv_bridge import CvBridge - from std_msgs.msg import Float32 -# from matplotlib.colors import LinearSegmentedColormap - class RadarNode: - """See doc/perception/lidar_distance_utility.md on - how to configute this node + """See doc/perception/radar_node.md on + how to configure this node """ def callback(self, data): - """Callback function, filters a PontCloud2 message - by restrictions defined in the launchfile. + """Process radar Point2Cloud data and publish minimum velocity. - Publishes a Depth image for the specified camera angle. - Each angle has do be delt with differently since the signs of the - coordinate system change with the view angle. + Extracts velocity information from radar data + and publishes the minimum velocity as a + Float32 message - :param data: a PointCloud2 + Args: + data: Point2Cloud message containing radar data with velocity field """ coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) @@ -40,8 +31,7 @@ def listener(self): Initializes the node and it's publishers """ # run simultaneously. - rospy.init_node("lidar_distance") - self.bridge = CvBridge() + rospy.init_node("radar_node") # publisher for radar dist_array self.dist_array_radar_publisher = rospy.Publisher( @@ -60,5 +50,5 @@ def listener(self): if __name__ == "__main__": - lidar_distance = RadarNode() - lidar_distance.listener() + radar_node = RadarNode() + radar_node.listener()