Dev: Update radar data

CHANGELOG.md

@@ -14,6 +14,7 @@
  * Added possibility to change gravity variable in imui sensor for the accelerometer
  * Fixed ROS2 native extension build error when ROS2 is installed in the system.
  * ROS2Native: Force fast-dds dependencies download to avoid build crash when boost_asio and tinyxml2 are not installed in Linux.
+ * Updated data coming from the radar. The speed is transmitted along both axes relative to the radar.
 
 ## CARLA 0.9.15
 

LibCarla/source/carla/sensor/data/RadarData.h

@@ -25,15 +25,16 @@ namespace s11n {
 namespace data {
 
   struct RadarDetection {
-    float velocity; // m/s
+    float velocity_x; // m/s
+    float velocity_y; // m/s
     float azimuth;  // rad
     float altitude; // rad
     float depth;    // m
   };
 
   class RadarData {
     static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");
-    static_assert(sizeof(float) * 4 == sizeof(RadarDetection), "Invalid RadarDetection size");
+    static_assert(sizeof(float) * 5 == sizeof(RadarDetection), "Invalid RadarDetection size");
 
   public:
     explicit RadarData() = default;

PythonAPI/carla/source/libcarla/SensorData.cpp

@@ -138,7 +138,8 @@ namespace data {
 
 
   std::ostream &operator<<(std::ostream &out, const RadarDetection &det) {
-    out << "RadarDetection(velocity=" << std::to_string(det.velocity)
+    out << "RadarDetection(velocity_x=" << std::to_string(det.velocity_x)
+        << ", velocity_y=" << std::to_string(det.velocity_y)
         << ", azimuth=" << std::to_string(det.azimuth)
         << ", altitude=" << std::to_string(det.altitude)
         << ", depth=" << std::to_string(det.depth)
@@ -523,7 +524,8 @@ void export_sensor_data() {
   ;
 
   class_<csd::RadarDetection>("RadarDetection")
-    .def_readwrite("velocity", &csd::RadarDetection::velocity)
+    .def_readwrite("velocity_x", &csd::RadarDetection::velocity_x)
+    .def_readwrite("velocity_y", &csd::RadarDetection::velocity_y)
     .def_readwrite("azimuth", &csd::RadarDetection::azimuth)
     .def_readwrite("altitude", &csd::RadarDetection::altitude)
     .def_readwrite("depth", &csd::RadarDetection::depth)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp

@@ -5,18 +5,20 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include <PxScene.h>
-
 #include "Carla.h"
 #include "Carla/Sensor/Radar.h"
 #include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
 #include "Kismet/KismetMathLibrary.h"
 #include "Runtime/Core/Public/Async/ParallelFor.h"
-
+#include "Containers/UnrealString.h"
 #include <compiler/disable-ue4-macros.h>
 #include "carla/geom/Math.h"
 #include "carla/ros2/ROS2.h"
 #include <compiler/enable-ue4-macros.h>
 
+#include <cmath>
+#include <string>
+
 FActorDefinition ARadar::GetSensorDefinition()
 {
   return UActorBlueprintFunctionLibrary::MakeRadarDefinition();
@@ -120,10 +122,14 @@ void ARadar::SendLineTraces(float DeltaTime)
   const FRotator& TransformRotator = ActorTransform.Rotator();
   const FVector& RadarLocation = GetActorLocation();
   const FVector& ForwardVector = GetActorForwardVector();
+  const FVector& RightVector = GetActorRightVector();
+
   const FVector TransformXAxis = ActorTransform.GetUnitAxis(EAxis::X);
   const FVector TransformYAxis = ActorTransform.GetUnitAxis(EAxis::Y);
   const FVector TransformZAxis = ActorTransform.GetUnitAxis(EAxis::Z);
 
+
+
   // Maximum radar radius in horizontal and vertical direction
   const float MaxRx = FMath::Tan(FMath::DegreesToRadians(HorizontalFOV * 0.5f)) * Range;
   const float MaxRy = FMath::Tan(FMath::DegreesToRadians(VerticalFOV * 0.5f)) * Range;
@@ -138,7 +144,7 @@ void ARadar::SendLineTraces(float DeltaTime)
     Rays[i].Hitted = false;
   }
 
-  FCriticalSection Mutex;
+  //FCriticalSection Mutex;
   GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
   {
     TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
@@ -170,7 +176,7 @@ void ARadar::SendLineTraces(float DeltaTime)
       if (Hitted && HittedActor.Get()) {
         Rays[idx].Hitted = true;
 
-        Rays[idx].RelativeVelocity = CalculateRelativeVelocity(OutHit, RadarLocation);
+        Rays[idx].RelativeVelocity = CalculateRelativeVelocity(OutHit, ForwardVector, RightVector, GetTransform().Inverse());
 
         Rays[idx].AzimuthAndElevation = FMath::GetAzimuthAndElevation (
           (EndLocation - RadarLocation).GetSafeNormal() * Range,
@@ -189,7 +195,8 @@ void ARadar::SendLineTraces(float DeltaTime)
   for (auto& ray : Rays) {
     if (ray.Hitted) {
       RadarData.WriteDetection({
-        ray.RelativeVelocity,
+        ray.RelativeVelocity.X,
+        ray.RelativeVelocity.Y,
         ray.AzimuthAndElevation.X,
         ray.AzimuthAndElevation.Y,
         ray.Distance
@@ -199,16 +206,23 @@ void ARadar::SendLineTraces(float DeltaTime)
 
 }
 
-float ARadar::CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation)
+FVector2D ARadar::CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& ForwardVector, const FVector& RightVector, const FTransform& ActorTransform)
 {
+
   constexpr float TO_METERS = 1e-2;
 
   const TWeakObjectPtr<AActor> HittedActor = OutHit.Actor;
   const FVector TargetVelocity = HittedActor->GetVelocity();
   const FVector TargetLocation = OutHit.ImpactPoint;
-  const FVector Direction = (TargetLocation - RadarLocation).GetSafeNormal();
+
   const FVector DeltaVelocity = (TargetVelocity - CurrentVelocity);
-  const float V = TO_METERS * FVector::DotProduct(DeltaVelocity, Direction);
+
+  FVector2D V(ActorTransform.TransformVector(DeltaVelocity));
+
+  V.X *= TO_METERS;
+  V.Y *= TO_METERS;
+
+
 
   return V;
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h

@@ -69,7 +69,7 @@ class CARLA_API ARadar : public ASensor
 
   void SendLineTraces(float DeltaTime);
 
-  float CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation);
+  FVector2D CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& ForvardVector, const FVector& RightVector, const FTransform& ActorTransform);
 
   FRadarData RadarData;
 
@@ -84,7 +84,7 @@ class CARLA_API ARadar : public ASensor
     float Radius;
     float Angle;
     bool Hitted;
-    float RelativeVelocity;
+    FVector2D RelativeVelocity;
     FVector2D AzimuthAndElevation;
     float Distance;
   };