CARLA
RayCastSemanticLidar.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 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 #include <cmath>
9 #include "Carla.h"
12 
14 #include "carla/geom/Math.h"
16 
17 #include "DrawDebugHelpers.h"
18 #include "Engine/CollisionProfile.h"
19 #include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
20 #include "Runtime/Core/Public/Async/ParallelFor.h"
21 
22 namespace crp = carla::rpc;
23 
25 {
26  return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast_semantic"));
27 }
28 
29 ARayCastSemanticLidar::ARayCastSemanticLidar(const FObjectInitializer& ObjectInitializer)
30  : Super(ObjectInitializer)
31 {
32  PrimaryActorTick.bCanEverTick = true;
33 }
34 
35 void ARayCastSemanticLidar::Set(const FActorDescription &ActorDescription)
36 {
37  Super::Set(ActorDescription);
38  FLidarDescription LidarDescription;
39  UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
40  Set(LidarDescription);
41 }
42 
43 void ARayCastSemanticLidar::Set(const FLidarDescription &LidarDescription)
44 {
45  Description = LidarDescription;
47  CreateLasers();
49 }
50 
52 {
53  const auto NumberOfLasers = Description.Channels;
54  check(NumberOfLasers > 0u);
55  const float DeltaAngle = NumberOfLasers == 1u ? 0.f :
57  static_cast<float>(NumberOfLasers - 1);
58  LaserAngles.Empty(NumberOfLasers);
59  for(auto i = 0u; i < NumberOfLasers; ++i)
60  {
61  const float VerticalAngle =
62  Description.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
63  LaserAngles.Emplace(VerticalAngle);
64  }
65 }
66 
67 void ARayCastSemanticLidar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
68 {
69  TRACE_CPUPROFILER_EVENT_SCOPE(ARayCastSemanticLidar::PostPhysTick);
70  SimulateLidar(DeltaTime);
71 
72  {
73  TRACE_CPUPROFILER_EVENT_SCOPE_STR("Send Stream");
74  auto DataStream = GetDataStream(*this);
75  DataStream.Send(*this, SemanticLidarData, DataStream.PopBufferFromPool());
76  }
77 }
78 
79 void ARayCastSemanticLidar::SimulateLidar(const float DeltaTime)
80 {
81  TRACE_CPUPROFILER_EVENT_SCOPE(ARayCastSemanticLidar::SimulateLidar);
82  const uint32 ChannelCount = Description.Channels;
83  const uint32 PointsToScanWithOneLaser =
84  FMath::RoundHalfFromZero(
85  Description.PointsPerSecond * DeltaTime / float(ChannelCount));
86 
87  if (PointsToScanWithOneLaser <= 0)
88  {
89  UE_LOG(
90  LogCarla,
91  Warning,
92  TEXT("%s: no points requested this frame, try increasing the number of points per second."),
93  *GetName());
94  return;
95  }
96 
97  check(ChannelCount == LaserAngles.Num());
98 
99  const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
101  const float AngleDistanceOfTick = Description.RotationFrequency * Description.HorizontalFov
102  * DeltaTime;
103  const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
104 
105  ResetRecordedHits(ChannelCount, PointsToScanWithOneLaser);
106  PreprocessRays(ChannelCount, PointsToScanWithOneLaser);
107 
108  GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
109  {
110  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
111  ParallelFor(ChannelCount, [&](int32 idxChannel) {
112  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelForTask);
113 
114  FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
115  TraceParams.bTraceComplex = true;
116  TraceParams.bReturnPhysicalMaterial = false;
117 
118  for (auto idxPtsOneLaser = 0u; idxPtsOneLaser < PointsToScanWithOneLaser; idxPtsOneLaser++) {
119  FHitResult HitResult;
120  const float VertAngle = LaserAngles[idxChannel];
121  const float HorizAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfLaserMeasure
122  * idxPtsOneLaser, Description.HorizontalFov) - Description.HorizontalFov / 2;
123  const bool PreprocessResult = RayPreprocessCondition[idxChannel][idxPtsOneLaser];
124 
125  if (PreprocessResult && ShootLaser(VertAngle, HorizAngle, HitResult, TraceParams)) {
126  WritePointAsync(idxChannel, HitResult);
127  }
128  };
129  });
130  }
131  GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
132 
133  FTransform ActorTransf = GetTransform();
134  ComputeAndSaveDetections(ActorTransf);
135 
136  const float HorizontalAngle = carla::geom::Math::ToRadians(
137  std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, Description.HorizontalFov));
138  SemanticLidarData.SetHorizontalAngle(HorizontalAngle);
139 }
140 
141 void ARayCastSemanticLidar::ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel) {
142  RecordedHits.resize(Channels);
143 
144  for (auto& hits : RecordedHits) {
145  hits.clear();
146  hits.reserve(MaxPointsPerChannel);
147  }
148 }
149 
150 void ARayCastSemanticLidar::PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel) {
151  RayPreprocessCondition.resize(Channels);
152 
153  for (auto& conds : RayPreprocessCondition) {
154  conds.clear();
155  conds.resize(MaxPointsPerChannel);
156  std::fill(conds.begin(), conds.end(), true);
157  }
158 }
159 
160 void ARayCastSemanticLidar::WritePointAsync(uint32_t channel, FHitResult &detection) {
161  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
162  DEBUG_ASSERT(GetChannelCount() > channel);
163  RecordedHits[channel].emplace_back(detection);
164 }
165 
166 void ARayCastSemanticLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
167  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
168  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
169  PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
171 
172  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
173  for (auto& hit : RecordedHits[idxChannel]) {
174  FSemanticDetection detection;
175  ComputeRawDetection(hit, SensorTransform, detection);
177  }
178  }
179 
181 }
182 
183 void ARayCastSemanticLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTransform& SensorTransf, FSemanticDetection& Detection) const
184 {
185  const FVector HitPoint = HitInfo.ImpactPoint;
186  Detection.point = SensorTransf.Inverse().TransformPosition(HitPoint);
187 
188  const FVector VecInc = - (HitPoint - SensorTransf.GetLocation()).GetSafeNormal();
189  Detection.cos_inc_angle = FVector::DotProduct(VecInc, HitInfo.ImpactNormal);
190 
191  const FActorRegistry &Registry = GetEpisode().GetActorRegistry();
192 
193  const AActor* actor = HitInfo.Actor.Get();
194  Detection.object_idx = 0;
195  Detection.object_tag = static_cast<uint32_t>(HitInfo.Component->CustomDepthStencilValue);
196 
197  if (actor != nullptr) {
198 
199  const FCarlaActor* view = Registry.FindCarlaActor(actor);
200  if(view)
201  Detection.object_idx = view->GetActorId();
202 
203  }
204  else {
205  UE_LOG(LogCarla, Warning, TEXT("Actor not valid %p!!!!"), actor);
206  }
207 }
208 
209 
210 bool ARayCastSemanticLidar::ShootLaser(const float VerticalAngle, const float HorizontalAngle, FHitResult& HitResult, FCollisionQueryParams& TraceParams) const
211 {
212  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
213 
214  FHitResult HitInfo(ForceInit);
215 
216  FTransform ActorTransf = GetTransform();
217  FVector LidarBodyLoc = ActorTransf.GetLocation();
218  FRotator LidarBodyRot = ActorTransf.Rotator();
219 
220  FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
221  FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
222  LaserRot,
223  LidarBodyRot
224  );
225 
226  const auto Range = Description.Range;
227  FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
228 
229  GetWorld()->ParallelLineTraceSingleByChannel(
230  HitInfo,
231  LidarBodyLoc,
232  EndTrace,
233  ECC_GameTraceChannel2,
234  TraceParams,
235  FCollisionResponseParams::DefaultResponseParam
236  );
237 
238  if (HitInfo.bBlockingHit) {
239  HitResult = HitInfo;
240  return true;
241  } else {
242  return false;
243  }
244 }
static FActorDefinition MakeLidarDefinition(const FString &Id)
std::vector< std::vector< FHitResult > > RecordedHits
A registry of all the Carla actors.
Definition: ActorRegistry.h:20
virtual void PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel)
Method that allow to preprocess if the rays will be traced.
carla::sensor::data::SemanticLidarData FSemanticLidarData
void WritePointAsync(uint32_t Channel, FHitResult &Detection)
Saving the hits the raycast returns per channel.
virtual void WriteChannelCount(std::vector< uint32_t > points_per_channel)
float Range
Measure distance in centimeters.
Helper class to store and serialize the data generated by a RawLidar.
void CreateLasers()
Creates a Laser for each channel.
FSemanticLidarData SemanticLidarData
float LowerFovLimit
Lower laser angle, counts from horizontal, negative values means under horizontal line...
void ComputeRawDetection(const FHitResult &HitInfo, const FTransform &SensorTransf, FSemanticDetection &Detection) const
Compute all raw detection information.
void SimulateLidar(const float DeltaTime)
Updates LidarMeasurement with the points read in DeltaTime.
A definition of a Carla Actor with all the variation and attributes.
FLidarDescription Description
virtual void ComputeAndSaveDetections(const FTransform &SensorTransform)
This method uses all the saved FHitResults, compute the RawDetections and then send it to the LidarDa...
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
const FActorRegistry & GetActorRegistry() const
Definition: CarlaEpisode.h:134
ARayCastSemanticLidar(const FObjectInitializer &ObjectInitializer)
void ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel)
Clear the recorded data structure.
virtual void ResetMemory(std::vector< uint32_t > points_per_channel)
float RotationFrequency
Lidar rotation frequency.
static void SetLidar(const FActorDescription &Description, FLidarDescription &Lidar)
A description of a Carla Actor with all its variation.
std::vector< std::vector< bool > > RayPreprocessCondition
virtual void PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime) override
virtual void WritePointSync(SemanticLidarDetection &detection)
uint32 Channels
Number of lasers.
float UpperFovLimit
Upper laser angle, counts from horizontal, positive values means above horizontal line...
static constexpr T ToRadians(T deg)
Definition: Math.h:43
bool ShootLaser(const float VerticalAngle, float HorizontalAngle, FHitResult &HitResult, FCollisionQueryParams &TraceParams) const
Shoot a laser ray-trace, return whether the laser hit something.
float HorizontalFov
Horizontal field of view.
FAsyncDataStream GetDataStream(const SensorT &Self)
Return the FDataStream associated with this sensor.
static constexpr T ToDegrees(T rad)
Definition: Math.h:37
IdType GetActorId() const
Definition: CarlaActor.h:80
FCarlaActor * FindCarlaActor(IdType Id)
Definition: ActorRegistry.h:69
virtual void Set(const FActorDescription &Description) override
uint32 PointsPerSecond
Points generated by all lasers per second.
std::vector< uint32_t > PointsPerChannel
static FActorDefinition GetSensorDefinition()
A view over an actor and its properties.
Definition: CarlaActor.h:23