From 979824d5c21964ca5a6f21a435f15b8bfcc9aa78 Mon Sep 17 00:00:00 2001
From: WilliaJ <1394350533@qq.com>
Date: Tue, 23 May 2023 14:08:00 +0800
Subject: [PATCH] fixbug:the radar message type does not match

---
 src/carla_cyber_bridge/radar.py  | 52 ++++++++++++++++++++++++--------
 src/carla_cyber_bridge/sensor.py |  5 +++
 2 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/src/carla_cyber_bridge/radar.py b/src/carla_cyber_bridge/radar.py
index 4d38728f..0f94ce9f 100755
--- a/src/carla_cyber_bridge/radar.py
+++ b/src/carla_cyber_bridge/radar.py
@@ -11,10 +11,12 @@
 """
 
 import numpy as np
+import math
 
-from carla_cyber_bridge.sensor import Sensor, create_cloud
+from carla_cyber_bridge.sensor import Sensor, create_cloud, create_radar
 
 from modules.drivers.proto.pointcloud_pb2 import PointXYZIT, PointCloud
+from modules.drivers.proto.conti_radar_pb2 import ContiRadar, ContiRadarObs
 
 
 class Radar(Sensor):
@@ -50,7 +52,7 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy
                                     carla_actor=carla_actor,
                                     synchronous_mode=synchronous_mode)
 
-        self.radar_writer = node.new_writer(self.get_topic_prefix(), PointCloud, qos_depth=10)
+        self.radar_writer = node.new_writer(self.get_topic_prefix(), ContiRadar, qos_depth=10)
         self.listen()
 
     def destroy(self):
@@ -71,15 +73,41 @@ def sensor_data_updated(self, carla_radar_measurement):
         :param carla_radar_measurement: carla Radar measurement object
         :type carla_radar_measurement: carla.RadarMeasurement
         """
-        points = []
+        # points = []
+        # for detection in carla_radar_measurement:
+        #     point = PointXYZIT()
+        #     point.x = detection.depth * np.cos(detection.azimuth) * np.cos(-detection.altitude)
+        #     point.y = detection.depth * np.sin(-detection.azimuth) * np.cos(detection.altitude)
+        #     point.z = detection.depth * np.sin(detection.altitude)
+        #     point.intensity = long(detection.depth)
+        #     points.append(point)
+        #
+        # radar_msg = create_cloud(self.get_msg_header(
+        #     timestamp=carla_radar_measurement.timestamp), points)
+        # self.radar_writer.write(radar_msg)
+
+        radars = []
+
         for detection in carla_radar_measurement:
-            point = PointXYZIT()
-            point.x = detection.depth * np.cos(detection.azimuth) * np.cos(-detection.altitude)
-            point.y = detection.depth * np.sin(-detection.azimuth) * np.cos(detection.altitude)
-            point.z = detection.depth * np.sin(detection.altitude)
-            point.intensity = long(detection.depth)
-            points.append(point)
-
-        radar_msg = create_cloud(self.get_msg_header(
-            timestamp=carla_radar_measurement.timestamp), points)
+            radarObs = ContiRadarObs()
+            azi_rad = math.radians(detection.azimuth)
+            x = detection.depth * math.cos(azi_rad)
+            y = detection.depth * math.sin(azi_rad)
+            # Shift the x and y coordinates so that they are centered around the radar position
+            x -= carla_radar_measurement.transform.location.x
+            y -= carla_radar_measurement.transform.location.y
+
+            velocity = detection.velocity
+            # Use the x-axis of the radar as the positive direction
+            vel_x = velocity * math.sin(-azi_rad)
+            vel_y = velocity * math.cos(-azi_rad)
+
+            radarObs.longitude_dist = x
+            radarObs.lateral_dist = y
+            radarObs.longitude_vel = vel_x
+            radarObs.lateral_vel = vel_y
+            radars.append(radarObs)
+
+        radar_msg = create_radar(self.get_msg_header(
+            timestamp=carla_radar_measurement.timestamp), radars)
         self.radar_writer.write(radar_msg)
diff --git a/src/carla_cyber_bridge/sensor.py b/src/carla_cyber_bridge/sensor.py
index 1733551e..27e90f0a 100755
--- a/src/carla_cyber_bridge/sensor.py
+++ b/src/carla_cyber_bridge/sensor.py
@@ -29,6 +29,7 @@
 
 from modules.drivers.proto.pointcloud_pb2 import PointXYZIT, PointCloud
 from modules.transform.proto.transform_pb2 import Transform, TransformStamped
+from modules.drivers.proto.conti_radar_pb2 import ContiRadar
 
 
 class Sensor(Actor):
@@ -286,3 +287,7 @@ def create_cloud(header, points):
                       point=points,
                       width=len(points),
                       height=1)
+def create_radar(header, contiobs):
+
+    return ContiRadar(header=header,
+                      contiobs=contiobs)