diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 10f79a80..67a9c2ab 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..04a9d59b --- /dev/null +++ b/code/perception/src/radar_node.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +import numpy as np +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Float32 + + +class RadarNode: + """See doc/perception/radar_node.md on + how to configure this node + """ + + def callback(self, data): + """Process radar Point2Cloud data and publish minimum velocity. + + Extracts velocity information from radar data + and publishes the minimum velocity as a + Float32 message + + Args: + data: Point2Cloud message containing radar data with velocity field + """ + + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + msg = np.min(coordinates["Velocity"]) + self.dist_array_radar_publisher.publish(msg) + + def listener(self): + """ + Initializes the node and it's publishers + """ + # run simultaneously. + rospy.init_node("radar_node") + + # publisher for radar dist_array + self.dist_array_radar_publisher = rospy.Publisher( + rospy.get_param("~image_distance_topic", "/paf/hero/Radar/velocity"), + Float32, + queue_size=10, + ) + + rospy.Subscriber( + rospy.get_param("~source_topic", "/carla/hero/RADAR"), + PointCloud2, + self.callback, + ) + + rospy.spin() + + +if __name__ == "__main__": + radar_node = RadarNode() + radar_node.listener() 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