CARLA
Radar.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include <PxScene.h>
8 
9 #include "Carla.h"
10 #include "Carla/Sensor/Radar.h"
12 #include "Kismet/KismetMathLibrary.h"
13 #include "Runtime/Core/Public/Async/ParallelFor.h"
14 
15 #include "carla/geom/Math.h"
16 
18 {
20 }
21 
22 ARadar::ARadar(const FObjectInitializer& ObjectInitializer)
23  : Super(ObjectInitializer)
24 {
25  PrimaryActorTick.bCanEverTick = true;
26 
27  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
28 
29  TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
30  TraceParams.bTraceComplex = true;
31  TraceParams.bReturnPhysicalMaterial = false;
32 
33 }
34 
35 void ARadar::Set(const FActorDescription &ActorDescription)
36 {
37  Super::Set(ActorDescription);
38  UActorBlueprintFunctionLibrary::SetRadar(ActorDescription, this);
39 }
40 
41 void ARadar::SetHorizontalFOV(float NewHorizontalFOV)
42 {
43  HorizontalFOV = NewHorizontalFOV;
44 }
45 
46 void ARadar::SetVerticalFOV(float NewVerticalFOV)
47 {
48  VerticalFOV = NewVerticalFOV;
49 }
50 
51 void ARadar::SetRange(float NewRange)
52 {
53  Range = NewRange;
54 }
55 
56 void ARadar::SetPointsPerSecond(int NewPointsPerSecond)
57 {
58  PointsPerSecond = NewPointsPerSecond;
60 }
61 
63 {
64  Super::BeginPlay();
65 
66  PrevLocation = GetActorLocation();
67 }
68 
69 void ARadar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
70 {
71  TRACE_CPUPROFILER_EVENT_SCOPE(ARadar::PostPhysTick);
72  CalculateCurrentVelocity(DeltaTime);
73 
74  RadarData.Reset();
75  SendLineTraces(DeltaTime);
76 
77  {
78  TRACE_CPUPROFILER_EVENT_SCOPE_STR("Send Stream");
79  auto DataStream = GetDataStream(*this);
80  DataStream.Send(*this, RadarData, DataStream.PopBufferFromPool());
81  }
82 }
83 
84 void ARadar::CalculateCurrentVelocity(const float DeltaTime)
85 {
86  const FVector RadarLocation = GetActorLocation();
87  CurrentVelocity = (RadarLocation - PrevLocation) / DeltaTime;
88  PrevLocation = RadarLocation;
89 }
90 
91 void ARadar::SendLineTraces(float DeltaTime)
92 {
93  TRACE_CPUPROFILER_EVENT_SCOPE(ARadar::SendLineTraces);
94  constexpr float TO_METERS = 1e-2;
95  const FTransform& ActorTransform = GetActorTransform();
96  const FRotator& TransformRotator = ActorTransform.Rotator();
97  const FVector& RadarLocation = GetActorLocation();
98  const FVector& ForwardVector = GetActorForwardVector();
99  const FVector TransformXAxis = ActorTransform.GetUnitAxis(EAxis::X);
100  const FVector TransformYAxis = ActorTransform.GetUnitAxis(EAxis::Y);
101  const FVector TransformZAxis = ActorTransform.GetUnitAxis(EAxis::Z);
102 
103  // Maximum radar radius in horizontal and vertical direction
104  const float MaxRx = FMath::Tan(FMath::DegreesToRadians(HorizontalFOV * 0.5f)) * Range;
105  const float MaxRy = FMath::Tan(FMath::DegreesToRadians(VerticalFOV * 0.5f)) * Range;
106  const int NumPoints = (int)(PointsPerSecond * DeltaTime);
107 
108  // Generate the parameters of the rays in a deterministic way
109  Rays.clear();
110  Rays.resize(NumPoints);
111  for (int i = 0; i < Rays.size(); i++) {
112  Rays[i].Radius = RandomEngine->GetUniformFloat();
113  Rays[i].Angle = RandomEngine->GetUniformFloatInRange(0.0f, carla::geom::Math::Pi2<float>());
114  Rays[i].Hitted = false;
115  }
116 
117  FCriticalSection Mutex;
118  GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
119  {
120  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
121  ParallelFor(NumPoints, [&](int32 idx) {
122  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelForTask);
123  FHitResult OutHit(ForceInit);
124  const float Radius = Rays[idx].Radius;
125  const float Angle = Rays[idx].Angle;
126 
127  float Sin, Cos;
128  FMath::SinCos(&Sin, &Cos, Angle);
129 
130  const FVector EndLocation = RadarLocation + TransformRotator.RotateVector({
131  Range,
132  MaxRx * Radius * Cos,
133  MaxRy * Radius * Sin
134  });
135 
136  const bool Hitted = GetWorld()->ParallelLineTraceSingleByChannel(
137  OutHit,
138  RadarLocation,
139  EndLocation,
140  ECC_GameTraceChannel2,
141  TraceParams,
142  FCollisionResponseParams::DefaultResponseParam
143  );
144 
145  const TWeakObjectPtr<AActor> HittedActor = OutHit.Actor;
146  if (Hitted && HittedActor.Get()) {
147  Rays[idx].Hitted = true;
148 
149  Rays[idx].RelativeVelocity = CalculateRelativeVelocity(OutHit, RadarLocation);
150 
151  Rays[idx].AzimuthAndElevation = FMath::GetAzimuthAndElevation (
152  (EndLocation - RadarLocation).GetSafeNormal() * Range,
153  TransformXAxis,
154  TransformYAxis,
155  TransformZAxis
156  );
157 
158  Rays[idx].Distance = OutHit.Distance * TO_METERS;
159  }
160  });
161  }
162  GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
163 
164  // Write the detections in the output structure
165  for (auto& ray : Rays) {
166  if (ray.Hitted) {
168  ray.RelativeVelocity,
169  ray.AzimuthAndElevation.X,
170  ray.AzimuthAndElevation.Y,
171  ray.Distance
172  });
173  }
174  }
175 
176 }
177 
178 float ARadar::CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation)
179 {
180  constexpr float TO_METERS = 1e-2;
181 
182  const TWeakObjectPtr<AActor> HittedActor = OutHit.Actor;
183  const FVector TargetVelocity = HittedActor->GetVelocity();
184  const FVector TargetLocation = OutHit.ImpactPoint;
185  const FVector Direction = (TargetLocation - RadarLocation).GetSafeNormal();
186  const FVector DeltaVelocity = (TargetVelocity - CurrentVelocity);
187  const float V = TO_METERS * FVector::DotProduct(DeltaVelocity, Direction);
188 
189  return V;
190 }
void SetHorizontalFOV(float NewHorizontalFOV)
Definition: Radar.cpp:41
float HorizontalFOV
Definition: Radar.h:58
virtual void PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime) override
Definition: Radar.cpp:69
FRadarData RadarData
Definition: Radar.h:74
FVector CurrentVelocity
Definition: Radar.h:78
float VerticalFOV
Definition: Radar.h:61
std::vector< RayData > Rays
Definition: Radar.h:92
void SetRange(float NewRange)
Definition: Radar.cpp:51
A definition of a Carla Actor with all the variation and attributes.
void BeginPlay() override
Definition: Radar.cpp:62
void SetResolution(uint32_t resolution)
Set a new resolution for the RadarData.
Definition: RadarData.h:44
void SendLineTraces(float DeltaTime)
Definition: Radar.cpp:91
float GetUniformFloatInRange(float Minimum, float Maximum)
Definition: RandomEngine.h:62
static void SetRadar(const FActorDescription &Description, ARadar *Radar)
void WriteDetection(RadarDetection detection)
Adds a new detection.
Definition: RadarData.h:64
void Set(const FActorDescription &Description) override
Definition: Radar.cpp:35
A description of a Carla Actor with all its variation.
ARadar(const FObjectInitializer &ObjectInitializer)
Definition: Radar.cpp:22
static FActorDefinition GetSensorDefinition()
Definition: Radar.cpp:17
float CalculateRelativeVelocity(const FHitResult &OutHit, const FVector &RadarLocation)
Definition: Radar.cpp:178
float GetUniformFloat()
Definition: RandomEngine.h:56
void CalculateCurrentVelocity(const float DeltaTime)
Definition: Radar.cpp:84
FAsyncDataStream GetDataStream(const SensorT &Self)
Return the FDataStream associated with this sensor.
FVector PrevLocation
Used to compute the velocity of the radar.
Definition: Radar.h:81
int PointsPerSecond
Definition: Radar.h:64
FCollisionQueryParams TraceParams
Definition: Radar.h:76
void Reset()
Deletes the current detections.
Definition: RadarData.h:59
float Range
Definition: Radar.h:55
void SetVerticalFOV(float NewVerticalFOV)
Definition: Radar.cpp:46
void SetPointsPerSecond(int NewPointsPerSecond)
Definition: Radar.cpp:56
URandomEngine * RandomEngine
Random Engine used to provide noise for sensor output.