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" 30 : Super(ObjectInitializer)
32 PrimaryActorTick.bCanEverTick =
true;
37 Super::Set(ActorDescription);
40 Set(LidarDescription);
54 check(NumberOfLasers > 0u);
55 const float DeltaAngle = NumberOfLasers == 1u ? 0.f :
57 static_cast<float>(NumberOfLasers - 1);
59 for(
auto i = 0u; i < NumberOfLasers; ++i)
61 const float VerticalAngle =
73 TRACE_CPUPROFILER_EVENT_SCOPE_STR(
"Send Stream");
83 const uint32 PointsToScanWithOneLaser =
84 FMath::RoundHalfFromZero(
87 if (PointsToScanWithOneLaser <= 0)
92 TEXT(
"%s: no points requested this frame, try increasing the number of points per second."),
103 const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
108 GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
110 TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
111 ParallelFor(ChannelCount, [&](int32 idxChannel) {
112 TRACE_CPUPROFILER_EVENT_SCOPE(ParallelForTask);
114 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT(
"Laser_Trace")),
true,
this);
115 TraceParams.bTraceComplex =
true;
116 TraceParams.bReturnPhysicalMaterial =
false;
118 for (
auto idxPtsOneLaser = 0u; idxPtsOneLaser < PointsToScanWithOneLaser; idxPtsOneLaser++) {
119 FHitResult HitResult;
121 const float HorizAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfLaserMeasure
125 if (PreprocessResult &&
ShootLaser(VertAngle, HorizAngle, HitResult, TraceParams)) {
131 GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
133 FTransform ActorTransf = GetTransform();
146 hits.reserve(MaxPointsPerChannel);
155 conds.resize(MaxPointsPerChannel);
156 std::fill(conds.begin(), conds.end(),
true);
161 TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
167 TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
185 const FVector HitPoint = HitInfo.ImpactPoint;
186 Detection.
point = SensorTransf.Inverse().TransformPosition(HitPoint);
188 const FVector VecInc = - (HitPoint - SensorTransf.GetLocation()).GetSafeNormal();
189 Detection.
cos_inc_angle = FVector::DotProduct(VecInc, HitInfo.ImpactNormal);
193 const AActor* actor = HitInfo.Actor.Get();
195 Detection.
object_tag =
static_cast<uint32_t
>(HitInfo.Component->CustomDepthStencilValue);
197 if (actor !=
nullptr) {
205 UE_LOG(LogCarla, Warning, TEXT(
"Actor not valid %p!!!!"), actor);
212 TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
214 FHitResult HitInfo(ForceInit);
216 FTransform ActorTransf = GetTransform();
217 FVector LidarBodyLoc = ActorTransf.GetLocation();
218 FRotator LidarBodyRot = ActorTransf.Rotator();
220 FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);
221 FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
227 FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
229 GetWorld()->ParallelLineTraceSingleByChannel(
233 ECC_GameTraceChannel2,
235 FCollisionResponseParams::DefaultResponseParam
238 if (HitInfo.bBlockingHit) {
static FActorDefinition MakeLidarDefinition(const FString &Id)
std::vector< std::vector< FHitResult > > RecordedHits
A registry of all the Carla actors.
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.
const UCarlaEpisode & GetEpisode() const
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)
const FActorRegistry & GetActorRegistry() const
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)
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.
TArray< float > LaserAngles
void SetHorizontalAngle(float angle)
static constexpr T ToDegrees(T rad)
IdType GetActorId() const
FCarlaActor * FindCarlaActor(IdType Id)
virtual void Set(const FActorDescription &Description) override
uint32 PointsPerSecond
Points generated by all lasers per second.
std::vector< uint32_t > PointsPerChannel
float GetHorizontalAngle() const
static FActorDefinition GetSensorDefinition()
A view over an actor and its properties.