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  SimulateLidar(DeltaTime);
70 
71  auto DataStream = GetDataStream(*this);
72  DataStream.Send(*this, SemanticLidarData, DataStream.PopBufferFromPool());
73 }
74 
75 void ARayCastSemanticLidar::SimulateLidar(const float DeltaTime)
76 {
77  const uint32 ChannelCount = Description.Channels;
78  const uint32 PointsToScanWithOneLaser =
79  FMath::RoundHalfFromZero(
80  Description.PointsPerSecond * DeltaTime / float(ChannelCount));
81 
82  if (PointsToScanWithOneLaser <= 0)
83  {
84  UE_LOG(
85  LogCarla,
86  Warning,
87  TEXT("%s: no points requested this frame, try increasing the number of points per second."),
88  *GetName());
89  return;
90  }
91 
92  check(ChannelCount == LaserAngles.Num());
93 
94  const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
96  const float AngleDistanceOfTick = Description.RotationFrequency * Description.HorizontalFov
97  * DeltaTime;
98  const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
99 
100  ResetRecordedHits(ChannelCount, PointsToScanWithOneLaser);
101  PreprocessRays(ChannelCount, PointsToScanWithOneLaser);
102 
103  GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
104  ParallelFor(ChannelCount, [&](int32 idxChannel) {
105  for (auto idxPtsOneLaser = 0u; idxPtsOneLaser < PointsToScanWithOneLaser; idxPtsOneLaser++) {
106  FHitResult HitResult;
107  const float VertAngle = LaserAngles[idxChannel];
108  const float HorizAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfLaserMeasure
109  * idxPtsOneLaser, Description.HorizontalFov) - Description.HorizontalFov / 2;
110  const bool PreprocessResult = RayPreprocessCondition[idxChannel][idxPtsOneLaser];
111 
112  if (PreprocessResult && ShootLaser(VertAngle, HorizAngle, HitResult)) {
113  WritePointAsync(idxChannel, HitResult);
114  }
115  };
116  });
117  GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
118 
119  FTransform ActorTransf = GetTransform();
120  ComputeAndSaveDetections(ActorTransf);
121 
122  const float HorizontalAngle = carla::geom::Math::ToRadians(
123  std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, Description.HorizontalFov));
124  SemanticLidarData.SetHorizontalAngle(HorizontalAngle);
125 }
126 
127 void ARayCastSemanticLidar::ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel) {
128  RecordedHits.resize(Channels);
129 
130  for (auto& hits : RecordedHits) {
131  hits.clear();
132  hits.reserve(MaxPointsPerChannel);
133  }
134 }
135 
136 void ARayCastSemanticLidar::PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel) {
137  RayPreprocessCondition.resize(Channels);
138 
139  for (auto& conds : RayPreprocessCondition) {
140  conds.clear();
141  conds.resize(MaxPointsPerChannel);
142  std::fill(conds.begin(), conds.end(), true);
143  }
144 }
145 
146 void ARayCastSemanticLidar::WritePointAsync(uint32_t channel, FHitResult &detection) {
147  DEBUG_ASSERT(GetChannelCount() > channel);
148  RecordedHits[channel].emplace_back(detection);
149 }
150 
151 void ARayCastSemanticLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
152  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
153  PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
155 
156  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
157  for (auto& hit : RecordedHits[idxChannel]) {
158  FSemanticDetection detection;
159  ComputeRawDetection(hit, SensorTransform, detection);
161  }
162  }
163 }
164 
165 void ARayCastSemanticLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTransform& SensorTransf, FSemanticDetection& Detection) const
166 {
167  const FVector HitPoint = HitInfo.ImpactPoint;
168  Detection.point = SensorTransf.Inverse().TransformPosition(HitPoint);
169 
170  const FVector VecInc = - (HitPoint - SensorTransf.GetLocation()).GetSafeNormal();
171  Detection.cos_inc_angle = FVector::DotProduct(VecInc, HitInfo.ImpactNormal);
172 
173  const FActorRegistry &Registry = GetEpisode().GetActorRegistry();
174 
175  const AActor* actor = HitInfo.Actor.Get();
176  Detection.object_idx = 0;
177  Detection.object_tag = static_cast<uint32_t>(HitInfo.Component->CustomDepthStencilValue);
178 
179  if (actor != nullptr) {
180 
181  const FActorView view = Registry.Find(actor);
182  if(view.IsValid())
183  Detection.object_idx = view.GetActorId();
184 
185  }
186  else {
187  UE_LOG(LogCarla, Warning, TEXT("Actor not valid %p!!!!"), actor);
188  }
189 }
190 
191 
192 bool ARayCastSemanticLidar::ShootLaser(const float VerticalAngle, const float HorizontalAngle, FHitResult& HitResult) const
193 {
194  FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
195  TraceParams.bTraceComplex = true;
196  TraceParams.bReturnPhysicalMaterial = false;
197 
198  FHitResult HitInfo(ForceInit);
199 
200  FTransform ActorTransf = GetTransform();
201  FVector LidarBodyLoc = ActorTransf.GetLocation();
202  FRotator LidarBodyRot = ActorTransf.Rotator();
203  FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
204  FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
205  LaserRot,
206  LidarBodyRot
207  );
208  const auto Range = Description.Range;
209  FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
210 
211  GetWorld()->LineTraceSingleByChannel(
212  HitInfo,
213  LidarBodyLoc,
214  EndTrace,
215  ECC_GameTraceChannel2,
216  TraceParams,
217  FCollisionResponseParams::DefaultResponseParam
218  );
219 
220  if (HitInfo.bBlockingHit) {
221  HitResult = HitInfo;
222  return true;
223  } else {
224  return false;
225  }
226 }
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 ResetSerPoints(std::vector< uint32_t > points_per_channel)
A view over an actor and its properties.
Definition: ActorView.h:14
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.
bool IsValid() const
Definition: ActorView.h:34
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.
IdType GetActorId() const
Definition: ActorView.h:39
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:142
ARayCastSemanticLidar(const FObjectInitializer &ObjectInitializer)
void ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel)
Clear the recorded data structure.
float RotationFrequency
Lidar rotation frequency.
bool ShootLaser(const float VerticalAngle, float HorizontalAngle, FHitResult &HitResult) const
Shoot a laser ray-trace, return whether the laser hit something.
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
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
FActorView Find(IdType Id) const
Definition: ActorRegistry.h:67
virtual void Set(const FActorDescription &Description) override
uint32 PointsPerSecond
Points generated by all lasers per second.
std::vector< uint32_t > PointsPerChannel
void SimulateLidar(float DeltaTime)
Updates LidarMeasurement with the points read in DeltaTime.
static FActorDefinition GetSensorDefinition()