From 8773d7492f84e3081fc625ea267eb12653b2ce31 Mon Sep 17 00:00:00 2001
From: Danryanh7 <danielryanh7@gmail.com>
Date: Fri, 31 May 2024 00:20:35 +0000
Subject: [PATCH] WIP adding launch file (still need to make node and port over
 changes)

---
 .../config/nuscenes_config.yaml               |  3 +
 .../launch/nuscenes_launch.py                 | 27 +++++++++
 .../radar_vis/src/radar_det.py                | 56 ++++++++++++-------
 3 files changed, 65 insertions(+), 21 deletions(-)
 create mode 100644 src/perception/radar_object_detection/config/nuscenes_config.yaml
 create mode 100644 src/perception/radar_object_detection/radar_object_detection_launch/launch/nuscenes_launch.py

diff --git a/src/perception/radar_object_detection/config/nuscenes_config.yaml b/src/perception/radar_object_detection/config/nuscenes_config.yaml
new file mode 100644
index 00000000..9991abdc
--- /dev/null
+++ b/src/perception/radar_object_detection/config/nuscenes_config.yaml
@@ -0,0 +1,3 @@
+radar_object_detection_node:
+  ros__parameters:
+    lidar_topic: /RADAR_TOP
\ No newline at end of file
diff --git a/src/perception/radar_object_detection/radar_object_detection_launch/launch/nuscenes_launch.py b/src/perception/radar_object_detection/radar_object_detection_launch/launch/nuscenes_launch.py
new file mode 100644
index 00000000..d73f3445
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/perception/radar_object_detection/radar_vis/src/radar_det.py b/src/perception/radar_object_detection/radar_vis/src/radar_det.py
index 7a19d678..a3593c7a 100644
--- a/src/perception/radar_object_detection/radar_vis/src/radar_det.py
+++ b/src/perception/radar_object_detection/radar_vis/src/radar_det.py
@@ -1,21 +1,35 @@
-class RadarVelocityDetectionNode(Node): 
-	def __init__(self): 
-		super().__init__('radar_velocity_detection_node') 
-		self.subscription = self.create_subscription( 
-		PointCloud2, 
-		'/RADAR_TOP', 
-		self.listener_callback,
-		 10)
-		self.publisher_ = self.create_publisher(Float32, 'velocity', 10) 
-		self.get_logger().info('Radar Velocity Detection Node has been started') 
-	def listener_callback(self, msg): 
-		points_array = ros2_numpy.point_cloud2.pointcloud2_to_array(msg) 
-		velocities = self.detect_velocities(points_array) 
-		avg_velocity = np.mean(velocities) if velocities.size > 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, points_array): 
-	# Assuming velocity is in fields 'vx' and 'vy' 
-		velocities = np.sqrt(points_array['vx']**2 + points_array['vy']**2) 
-		return velocities
\ No newline at end of file
+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()