[DRAFT] Radar Velocity Detection

src/perception/radar_object_detection/config/nuscenes_config.yaml

@@ -0,0 +1,3 @@
+radar_object_detection_node:
+  ros__parameters:
+    lidar_topic: /RADAR_TOP

src/perception/radar_object_detection/radar_object_detection_launch/launch/nuscenes_launch.py

@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    config = os.path.join(
+        get_package_share_directory('radar_object_detection'),
+        'config',
+        'nuscenes_config.yaml'
+    )
+
+    # nodes
+    radar_detection = Node(
+        package='radar_vis',
+        executable='radar_vis_node',
+        name='radar_velocity_detection_node',
+        parameters=[config],
+        arguments=['--ros-args', '--log-level', 'info']
+    )
+
+    # finalize
+    ld.add_action(radar_detection)
+
+    return ld

src/perception/radar_object_detection/radar_vis/src/radar_det.py

@@ -0,0 +1,35 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+from radar_velocity_detection.msg import RadarDetections
+import numpy as np
+
+class RadarVelocityDetectionNode(Node):
+    def __init__(self):
+        super().__init__('radar_velocity_detection_node')
+        self.subscription = self.create_subscription(
+            RadarDetections,
+            '/RADAR_TOP',
+            self.listener_callback,
+            10)
+        self.publisher_ = self.create_publisher(Float32, 'velocity', 10)
+
+    def listener_callback(self, msg):
+        velocities = self.detect_velocities(msg.detections)
+        avg_velocity = np.mean(velocities) if len(velocities) > 0 else 0.0
+        self.publisher_.publish(Float32(data=avg_velocity))
+        self.get_logger().info(f'Average velocity: {avg_velocity:.2f} m/s')
+
+    def detect_velocities(self, detections):
+        velocities = [np.sqrt(d.vx**2 + d.vy**2) for d in detections]
+        return velocities
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RadarVelocityDetectionNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()