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"
15 #include "carla/ros2/ROS2.h"
17 
18 #include "DrawDebugHelpers.h"
19 #include "Engine/CollisionProfile.h"
20 #include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
21 #include "Runtime/Core/Public/Async/ParallelFor.h"
22 
23 namespace crp = carla::rpc;
24 
26 {
27  return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast_semantic"));
28 }
29 
30 ARayCastSemanticLidar::ARayCastSemanticLidar(const FObjectInitializer& ObjectInitializer)
31  : Super(ObjectInitializer)
32 {
33  PrimaryActorTick.bCanEverTick = true;
34 }
35 
36 void ARayCastSemanticLidar::Set(const FActorDescription &ActorDescription)
37 {
38  Super::Set(ActorDescription);
39  FLidarDescription LidarDescription;
40  UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
41  Set(LidarDescription);
42 }
43 
44 void ARayCastSemanticLidar::Set(const FLidarDescription &LidarDescription)
45 {
46  Description = LidarDescription;
48  CreateLasers();
50 }
51 
53 {
54  const auto NumberOfLasers = Description.Channels;
55  check(NumberOfLasers > 0u);
56  const float DeltaAngle = NumberOfLasers == 1u ? 0.f :
58  static_cast<float>(NumberOfLasers - 1);
59  LaserAngles.Empty(NumberOfLasers);
60  for(auto i = 0u; i < NumberOfLasers; ++i)
61  {
62  const float VerticalAngle =
63  Description.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
64  LaserAngles.Emplace(VerticalAngle);
65  }
66 }
67 
68 void ARayCastSemanticLidar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
69 {
70  TRACE_CPUPROFILER_EVENT_SCOPE(ARayCastSemanticLidar::PostPhysTick);
71  SimulateLidar(DeltaTime);
72 
73  auto DataStream = GetDataStream(*this);
74  auto SensorTransform = DataStream.GetSensorTransform();
75  {
76  TRACE_CPUPROFILER_EVENT_SCOPE_STR("Send Stream");
77  DataStream.SerializeAndSend(*this, SemanticLidarData, DataStream.PopBufferFromPool());
78  }
79  // ROS2
80  #if defined(WITH_ROS2)
81  auto ROS2 = carla::ros2::ROS2::GetInstance();
82  if (ROS2->IsEnabled())
83  {
84  TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send");
86  AActor* ParentActor = GetAttachParentActor();
87  if (ParentActor)
88  {
89  FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform());
90  ROS2->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, SemanticLidarData, this);
91  }
92  else
93  {
94  ROS2->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, SensorTransform, SemanticLidarData, this);
95  }
96  }
97  #endif
98 }
99 
100 void ARayCastSemanticLidar::SimulateLidar(const float DeltaTime)
101 {
102  TRACE_CPUPROFILER_EVENT_SCOPE(ARayCastSemanticLidar::SimulateLidar);
103  const uint32 ChannelCount = Description.Channels;
104  const uint32 PointsToScanWithOneLaser =
105  FMath::RoundHalfFromZero(
106  Description.PointsPerSecond * DeltaTime / float(ChannelCount));
107 
108  if (PointsToScanWithOneLaser <= 0)
109  {
110  UE_LOG(
111  LogCarla,
112  Warning,
113  TEXT("%s: no points requested this frame, try increasing the number of points per second."),
114  *GetName());
115  return;
116  }
117 
118  check(ChannelCount == LaserAngles.Num());
119 
120  const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
122  const float AngleDistanceOfTick = Description.RotationFrequency * Description.HorizontalFov
123  * DeltaTime;
124  const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
125 
126  ResetRecordedHits(ChannelCount, PointsToScanWithOneLaser);
127  PreprocessRays(ChannelCount, PointsToScanWithOneLaser);
128 
129  GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
130  {
131  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
132  ParallelFor(ChannelCount, [&](int32 idxChannel) {
133  TRACE_CPUPROFILER_EVENT_SCOPE(ParallelForTask);
134 
135  FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
136  TraceParams.bTraceComplex = true;
137  TraceParams.bReturnPhysicalMaterial = false;
138 
139  for (auto idxPtsOneLaser = 0u; idxPtsOneLaser < PointsToScanWithOneLaser; idxPtsOneLaser++) {
140  FHitResult HitResult;
141  const float VertAngle = LaserAngles[idxChannel];
142  const float HorizAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfLaserMeasure
143  * idxPtsOneLaser, Description.HorizontalFov) - Description.HorizontalFov / 2;
144  const bool PreprocessResult = RayPreprocessCondition[idxChannel][idxPtsOneLaser];
145 
146  if (PreprocessResult && ShootLaser(VertAngle, HorizAngle, HitResult, TraceParams)) {
147  WritePointAsync(idxChannel, HitResult);
148  }
149  };
150  });
151  }
152  GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
153 
154  FTransform ActorTransf = GetTransform();
155  ComputeAndSaveDetections(ActorTransf);
156 
157  const float HorizontalAngle = carla::geom::Math::ToRadians(
158  std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, Description.HorizontalFov));
159  SemanticLidarData.SetHorizontalAngle(HorizontalAngle);
160 }
161 
162 void ARayCastSemanticLidar::ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel) {
163  RecordedHits.resize(Channels);
164 
165  for (auto& hits : RecordedHits) {
166  hits.clear();
167  hits.reserve(MaxPointsPerChannel);
168  }
169 }
170 
171 void ARayCastSemanticLidar::PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel) {
172  RayPreprocessCondition.resize(Channels);
173 
174  for (auto& conds : RayPreprocessCondition) {
175  conds.clear();
176  conds.resize(MaxPointsPerChannel);
177  std::fill(conds.begin(), conds.end(), true);
178  }
179 }
180 
181 void ARayCastSemanticLidar::WritePointAsync(uint32_t channel, FHitResult &detection) {
182  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
183  DEBUG_ASSERT(GetChannelCount() > channel);
184  RecordedHits[channel].emplace_back(detection);
185 }
186 
187 void ARayCastSemanticLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
188  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
189  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
190  PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
192 
193  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
194  for (auto& hit : RecordedHits[idxChannel]) {
195  FSemanticDetection detection;
196  ComputeRawDetection(hit, SensorTransform, detection);
198  }
199  }
200 
202 }
203 
204 void ARayCastSemanticLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTransform& SensorTransf, FSemanticDetection& Detection) const
205 {
206  const FVector HitPoint = HitInfo.ImpactPoint;
207  Detection.point = SensorTransf.Inverse().TransformPosition(HitPoint);
208 
209  const FVector VecInc = - (HitPoint - SensorTransf.GetLocation()).GetSafeNormal();
210  Detection.cos_inc_angle = FVector::DotProduct(VecInc, HitInfo.ImpactNormal);
211 
212  const FActorRegistry &Registry = GetEpisode().GetActorRegistry();
213 
214  const AActor* actor = HitInfo.Actor.Get();
215  Detection.object_idx = 0;
216  Detection.object_tag = static_cast<uint32_t>(HitInfo.Component->CustomDepthStencilValue);
217 
218  if (actor != nullptr) {
219 
220  const FCarlaActor* view = Registry.FindCarlaActor(actor);
221  if(view)
222  Detection.object_idx = view->GetActorId();
223 
224  }
225  else {
226  UE_LOG(LogCarla, Warning, TEXT("Actor not valid %p!!!!"), actor);
227  }
228 }
229 
230 
231 bool ARayCastSemanticLidar::ShootLaser(const float VerticalAngle, const float HorizontalAngle, FHitResult& HitResult, FCollisionQueryParams& TraceParams) const
232 {
233  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
234 
235  FHitResult HitInfo(ForceInit);
236 
237  FTransform ActorTransf = GetTransform();
238  FVector LidarBodyLoc = ActorTransf.GetLocation();
239  FRotator LidarBodyRot = ActorTransf.Rotator();
240 
241  FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
242  FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
243  LaserRot,
244  LidarBodyRot
245  );
246 
247  const auto Range = Description.Range;
248  FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
249 
250  GetWorld()->ParallelLineTraceSingleByChannel(
251  HitInfo,
252  LidarBodyLoc,
253  EndTrace,
254  ECC_GameTraceChannel2,
255  TraceParams,
256  FCollisionResponseParams::DefaultResponseParam
257  );
258 
259  if (HitInfo.bBlockingHit) {
260  HitResult = HitInfo;
261  return true;
262  } else {
263  return false;
264  }
265 }
auto GetToken() const
Return the token that allows subscribing to this sensor&#39;s stream.
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:155
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...
const auto & get_stream_id() const
Definition: detail/Token.h:116
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 std::shared_ptr< ROS2 > GetInstance()
Definition: ROS2.h:51
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
carla::streaming::detail::token_type token_type
static FActorDefinition GetSensorDefinition()
A view over an actor and its properties.
Definition: CarlaActor.h:23